1/*
2 * File: motor.c
3 *
4 * Code generated for Simulink model 'motor'.
5 *
6 * Model version : 1.787
7 * Simulink Coder version : 8.9 (R2015b) 13-Aug-2015
8 * C/C++ source code generated on : Wed Oct 23 22:04:43 2019
9 *
10 * Target selection: ert.tlc
11 * Embedded hardware selection: ARM Compatible->ARM Cortex
12 * Code generation objectives: Unspecified
13 * Validation result: Not run
14 */
15
16#include "motor.h"
17#include "motor_private.h"
18
19/* Named constants for Chart: '<Root>/Chart' */
20#define motor_IN_Algorithm ((uint8_T)1U)
21#define motor_IN_Calibration ((uint8_T)1U)
22#define motor_IN_Check_Reset ((uint8_T)1U)
23#define motor_IN_Close ((uint8_T)1U)
24#define motor_IN_Close_Wait ((uint8_T)2U)
25#define motor_IN_Defualt ((uint8_T)1U)
26#define motor_IN_Defualt1 ((uint8_T)1U)
27#define motor_IN_Defult ((uint8_T)1U)
28#define motor_IN_Defult1 ((uint8_T)2U)
29#define motor_IN_Defult2 ((uint8_T)3U)
30#define motor_IN_Dspace_Test ((uint8_T)2U)
31#define motor_IN_Elevation_Test ((uint8_T)3U)
32#define motor_IN_Error_Check ((uint8_T)1U)
33#define motor_IN_HY ((uint8_T)2U)
34#define motor_IN_HYDG_Close ((uint8_T)1U)
35#define motor_IN_HY_f ((uint8_T)1U)
36#define motor_IN_Initialize ((uint8_T)2U)
37#define motor_IN_Int ((uint8_T)1U)
38#define motor_IN_Int1 ((uint8_T)2U)
39#define motor_IN_Int1_d ((uint8_T)3U)
40#define motor_IN_Int2 ((uint8_T)4U)
41#define motor_IN_Int2_i ((uint8_T)3U)
42#define motor_IN_Int3 ((uint8_T)3U)
43#define motor_IN_Int3_l ((uint8_T)4U)
44#define motor_IN_Int4 ((uint8_T)4U)
45#define motor_IN_Int4_b ((uint8_T)5U)
46#define motor_IN_Int5 ((uint8_T)5U)
47#define motor_IN_Int5_a ((uint8_T)6U)
48#define motor_IN_Int6 ((uint8_T)6U)
49#define motor_IN_Int6_m ((uint8_T)7U)
50#define motor_IN_Int7 ((uint8_T)8U)
51#define motor_IN_Int8 ((uint8_T)9U)
52#define motor_IN_Int_k ((uint8_T)2U)
53#define motor_IN_Limit_Down_Test ((uint8_T)4U)
54#define motor_IN_Limit_Down_Test_f ((uint8_T)3U)
55#define motor_IN_Limit_Up_Test ((uint8_T)5U)
56#define motor_IN_Limit_Up_Test_k ((uint8_T)4U)
57#define motor_IN_NO_ACTIVE_CHILD ((uint8_T)0U)
58#define motor_IN_Normal_Mode ((uint8_T)3U)
59#define motor_IN_P1 ((uint8_T)1U)
60#define motor_IN_P10 ((uint8_T)2U)
61#define motor_IN_P11 ((uint8_T)3U)
62#define motor_IN_P12 ((uint8_T)4U)
63#define motor_IN_P2 ((uint8_T)4U)
64#define motor_IN_P2_a ((uint8_T)5U)
65#define motor_IN_P3 ((uint8_T)5U)
66#define motor_IN_P3_k ((uint8_T)6U)
67#define motor_IN_P4 ((uint8_T)6U)
68#define motor_IN_P4_o ((uint8_T)7U)
69#define motor_IN_P5 ((uint8_T)7U)
70#define motor_IN_P5_n ((uint8_T)8U)
71#define motor_IN_P8 ((uint8_T)8U)
72#define motor_IN_P8_e ((uint8_T)9U)
73#define motor_IN_PID_parameters1 ((uint8_T)1U)
74#define motor_IN_PID_parameters2 ((uint8_T)2U)
75#define motor_IN_PID_parameters3 ((uint8_T)3U)
76#define motor_IN_Parameters_Reset ((uint8_T)2U)
77#define motor_IN_Parameters_Reset0 ((uint8_T)3U)
78#define motor_IN_QDQ_BJ ((uint8_T)1U)
79#define motor_IN_QDQ_GZ ((uint8_T)2U)
80#define motor_IN_RUN ((uint8_T)1U)
81#define motor_IN_Ready ((uint8_T)1U)
82#define motor_IN_Ready_Run ((uint8_T)2U)
83#define motor_IN_Showing_Mode ((uint8_T)4U)
84#define motor_IN_Sleeping_protect_1 ((uint8_T)1U)
85#define motor_IN_Sleeping_protect_1_h ((uint8_T)2U)
86#define motor_IN_Sleeping_protect_2 ((uint8_T)2U)
87#define motor_IN_Sleeping_protect_2_m ((uint8_T)3U)
88#define motor_IN_Sleeping_protect_3 ((uint8_T)3U)
89#define motor_IN_Sleeping_protect_3_f ((uint8_T)4U)
90#define motor_IN_Start ((uint8_T)2U)
91#define motor_IN_Test_Mode ((uint8_T)5U)
92#define motor_IN_XHHY ((uint8_T)3U)
93#define motor_IN_XHHY_Close ((uint8_T)2U)
94#define motor_IN_XHHY_Error ((uint8_T)5U)
95#define motor_IN_XHHY_Error1 ((uint8_T)4U)
96#define motor_IN_XHHY_Run ((uint8_T)5U)
97#define motor_IN_XHHY_Run1 ((uint8_T)6U)
98#define motor_IN_XHHY_Run1_d ((uint8_T)7U)
99#define motor_IN_XHHY_Run_e ((uint8_T)6U)
100#define motor_IN_XHHY_Wait ((uint8_T)7U)
101#define motor_IN_XHHY_Wait1 ((uint8_T)8U)
102#define motor_IN_XHHY_Wait2 ((uint8_T)9U)
103#define motor_IN_XHHY_Wait_g ((uint8_T)8U)
104#define motor_IN_XHHY_m ((uint8_T)5U)
105#define motor_IN_XHHY_m1 ((uint8_T)2U)
106#define motor_IN_XHZY ((uint8_T)4U)
107#define motor_IN_XHZY_Close ((uint8_T)3U)
108#define motor_IN_XHZY_Close1 ((uint8_T)6U)
109#define motor_IN_XHZY_Close1_f ((uint8_T)3U)
110#define motor_IN_XHZY_Close_c ((uint8_T)5U)
111#define motor_IN_XHZY_Close_cm ((uint8_T)2U)
112#define motor_IN_XHZY_Error ((uint8_T)7U)
113#define motor_IN_XHZY_Error1 ((uint8_T)4U)
114#define motor_IN_XHZY_Error_f ((uint8_T)4U)
115#define motor_IN_XHZY_Error_fb ((uint8_T)5U)
116#define motor_IN_XHZY_Run ((uint8_T)5U)
117#define motor_IN_XHZY_Run1 ((uint8_T)6U)
118#define motor_IN_XHZY_Run1_a ((uint8_T)7U)
119#define motor_IN_XHZY_Run_i ((uint8_T)6U)
120#define motor_IN_XHZY_Wait ((uint8_T)7U)
121#define motor_IN_XHZY_Wait_l ((uint8_T)8U)
122#define motor_IN_XHZY_p ((uint8_T)6U)
123#define motor_IN_XHZY_pv ((uint8_T)3U)
124#define motor_IN_defult ((uint8_T)6U)
125#define motor_IN_defult1 ((uint8_T)1U)
126#define motor_IN_defult1_k ((uint8_T)2U)
127#define motor_IN_defult2 ((uint8_T)2U)
128#define motor_IN_defult2_e ((uint8_T)3U)
129#define motor_IN_defult3 ((uint8_T)4U)
130#define motor_IN_defult4 ((uint8_T)5U)
131#define motor_IN_defult_b ((uint8_T)3U)
132#define motor_IN_defult_bq ((uint8_T)1U)
133#define motor_IN_defult_bqy ((uint8_T)2U)
134#define motor_IN_hy ((uint8_T)4U)
135#define motor_IN_p1 ((uint8_T)1U)
136#define motor_IN_p10 ((uint8_T)10U)
137#define motor_IN_p11 ((uint8_T)9U)
138#define motor_IN_p11_b ((uint8_T)11U)
139#define motor_IN_p12 ((uint8_T)10U)
140#define motor_IN_p1_a ((uint8_T)3U)
141#define motor_IN_p1_aq ((uint8_T)4U)
142#define motor_IN_p1_aqc ((uint8_T)2U)
143#define motor_IN_p2 ((uint8_T)2U)
144#define motor_IN_p2_k ((uint8_T)5U)
145#define motor_IN_p2_kb ((uint8_T)3U)
146#define motor_IN_p2_kbn ((uint8_T)1U)
147#define motor_IN_p3 ((uint8_T)3U)
148#define motor_IN_p3_e ((uint8_T)6U)
149#define motor_IN_p3_ex ((uint8_T)4U)
150#define motor_IN_p3_exj ((uint8_T)2U)
151#define motor_IN_p4 ((uint8_T)4U)
152#define motor_IN_p4_n ((uint8_T)7U)
153#define motor_IN_p4_nv ((uint8_T)2U)
154#define motor_IN_p4_nvu ((uint8_T)3U)
155#define motor_IN_p5 ((uint8_T)5U)
156#define motor_IN_p5_m ((uint8_T)3U)
157#define motor_IN_p6 ((uint8_T)6U)
158#define motor_IN_p7 ((uint8_T)7U)
159#define motor_IN_r2 ((uint8_T)8U)
160#define motor_IN_xhhy ((uint8_T)5U)
161#define motor_IN_xhzy ((uint8_T)6U)
162
163/* Block signals (auto storage) */
164BlockIO_motor motor_B;
165
166/* Block states (auto storage) */
167D_Work_motor motor_DWork;
168
169/* External inputs (root inport signals with auto storage) */
170ExternalInputs_motor motor_U;
171
172/* External outputs (root outports fed by signals with auto storage) */
173ExternalOutputs_motor motor_Y;
174
175/* Real-time model */
176RT_MODEL_motor motor_M_;
177RT_MODEL_motor *const motor_M = &motor_M_;
178
179/* Forward declaration for local functions */
180static uint8_T motor_Temp_Up_Check(uint16_T In1, uint16_T In2);
181static uint8_T motor_Temp_Down_Check(uint16_T In1, uint16_T In2);
182static void motor_defult(void);
183static void motor_XHZY_as(void);
184static void motor_Limit_Down_Test(void);
185static void motor_XHZY_a(void);
186static void motor_Limit_Up_Test(void);
187static void motor_exit_internal_Test_Mode(void);
188static uint8_T motor_Init_Result_Check(uint8_T In1, uint8_T In2, real_T In3);
189static void motor_hy(void);
190static void motor_xhhy(void);
191static void motor_Initialize(void);
192static void motor_enter_atomic_XHZY_Run(void);
193static void motor_enter_atomic_XHZY_Run1(void);
194static void motor_exit_internal_Normal_Mode(void);
195static void motor_HY(void);
196static void motor_XHHY(void);
197static void motor_Algorithm_a(void);
198static void motor_RUN(void);
199static void motor_Normal_Mode(void);
200static void moto_exit_internal_Showing_Mode(void);
201static void enter_internal_Limit_Down_Test(void);
202static void mo_enter_internal_Limit_Up_Test(void);
203static void motor_XHZY(void);
204static void motor_XHZY_asn(void);
205static void motor_Limit_Down_Test_b(void);
206static void motor_XHZY_asnq(void);
207static void motor_Limit_Up_Test_a(void);
208static void motor_XHHY_n(void);
209static void motor_Algorithm_ac(void);
210static void motor_Showing_Mode(void);
211static void motor_Algorithm(void);
212static void motor_M_Run(void);
213static void motor_Ready_Run(void);
214void sMultiWordMul(const uint32_T u1[], int32_T n1, const uint32_T u2[], int32_T
215 n2, uint32_T y[], int32_T n)
216{
217 int32_T i;
218 int32_T j;
219 int32_T k;
220 int32_T nj;
221 uint32_T u1i;
222 uint32_T yk;
223 uint32_T a1;
224 uint32_T a0;
225 uint32_T b1;
226 uint32_T w10;
227 uint32_T w01;
228 uint32_T cb;
229 boolean_T isNegative1;
230 boolean_T isNegative2;
231 uint32_T cb1;
232 uint32_T cb2;
233 isNegative1 = ((u1[n1 - 1] & 2147483648U) != 0U);
234 isNegative2 = ((u2[n2 - 1] & 2147483648U) != 0U);
235 cb1 = 1U;
236
237 /* Initialize output to zero */
238 for (k = 0; k < n; k++) {
239 y[k] = 0U;
240 }
241
242 for (i = 0; i < n1; i++) {
243 cb = 0U;
244 u1i = u1[i];
245 if (isNegative1) {
246 u1i = ~u1i + cb1;
247 cb1 = (uint32_T)(u1i < cb1);
248 }
249
250 a1 = u1i >> 16U;
251 a0 = u1i & 65535U;
252 cb2 = 1U;
253 k = n - i;
254 nj = n2 <= k ? n2 : k;
255 k = i;
256 for (j = 0; j < nj; j++) {
257 yk = y[k];
258 u1i = u2[j];
259 if (isNegative2) {
260 u1i = ~u1i + cb2;
261 cb2 = (uint32_T)(u1i < cb2);
262 }
263
264 b1 = u1i >> 16U;
265 u1i &= 65535U;
266 w10 = a1 * u1i;
267 w01 = a0 * b1;
268 yk += cb;
269 cb = (uint32_T)(yk < cb);
270 u1i *= a0;
271 yk += u1i;
272 cb += (yk < u1i);
273 u1i = w10 << 16U;
274 yk += u1i;
275 cb += (yk < u1i);
276 u1i = w01 << 16U;
277 yk += u1i;
278 cb += (yk < u1i);
279 y[k] = yk;
280 cb += w10 >> 16U;
281 cb += w01 >> 16U;
282 cb += a1 * b1;
283 k++;
284 }
285
286 if (k < n) {
287 y[k] = cb;
288 }
289 }
290
291 /* Apply sign */
292 if (isNegative1 != isNegative2) {
293 cb = 1U;
294 for (k = 0; k < n; k++) {
295 yk = ~y[k] + cb;
296 y[k] = yk;
297 cb = (uint32_T)(yk < cb);
298 }
299 }
300}
301
302real_T sMultiWord2Double(const uint32_T u1[], int32_T n1, int32_T e1)
303{
304 real_T y;
305 int32_T i;
306 int32_T exp_0;
307 uint32_T u1i;
308 uint32_T cb;
309 y = 0.0;
310 exp_0 = e1;
311 if ((u1[n1 - 1] & 2147483648U) != 0U) {
312 cb = 1U;
313 for (i = 0; i < n1; i++) {
314 u1i = ~u1[i];
315 cb += u1i;
316 y -= ldexp(cb, exp_0);
317 cb = (uint32_T)(cb < u1i);
318 exp_0 += 32;
319 }
320 } else {
321 for (i = 0; i < n1; i++) {
322 y += ldexp(u1[i], exp_0);
323 exp_0 += 32;
324 }
325 }
326
327 return y;
328}
329
330boolean_T sMultiWordGt(const uint32_T u1[], const uint32_T u2[], int32_T n)
331{
332 return sMultiWordCmp(u1, u2, n) > 0 ? (int32_T)1 : (int32_T)0;
333}
334
335int32_T sMultiWordCmp(const uint32_T u1[], const uint32_T u2[], int32_T n)
336{
337 int32_T y;
338 uint32_T su1;
339 uint32_T su2;
340 int32_T i;
341 su1 = u1[n - 1] & 2147483648U;
342 su2 = u2[n - 1] & 2147483648U;
343 if ((su1 ^ su2) != 0U) {
344 y = su1 != 0U ? -1 : 1;
345 } else {
346 y = 0;
347 i = n;
348 while ((y == 0) && (i > 0)) {
349 i--;
350 su1 = u1[i];
351 su2 = u2[i];
352 if (su1 != su2) {
353 y = su1 > su2 ? 1 : -1;
354 }
355 }
356 }
357
358 return y;
359}
360
361boolean_T sMultiWordLt(const uint32_T u1[], const uint32_T u2[], int32_T n)
362{
363 return sMultiWordCmp(u1, u2, n) < 0 ? (int32_T)1 : (int32_T)0;
364}
365
366void mul_wide_s32(int32_T in0, int32_T in1, uint32_T *ptrOutBitsHi, uint32_T
367 *ptrOutBitsLo)
368{
369 uint32_T absIn0;
370 uint32_T absIn1;
371 uint32_T in0Lo;
372 uint32_T in0Hi;
373 uint32_T in1Hi;
374 uint32_T productHiLo;
375 uint32_T productLoHi;
376 absIn0 = (uint32_T)(in0 < 0 ? -in0 : in0);
377 absIn1 = (uint32_T)(in1 < 0 ? -in1 : in1);
378 in0Hi = absIn0 >> 16U;
379 in0Lo = absIn0 & 65535U;
380 in1Hi = absIn1 >> 16U;
381 absIn0 = absIn1 & 65535U;
382 productHiLo = in0Hi * absIn0;
383 productLoHi = in0Lo * in1Hi;
384 absIn0 *= in0Lo;
385 absIn1 = 0U;
386 in0Lo = (productLoHi << 16U) + absIn0;
387 if (in0Lo < absIn0) {
388 absIn1 = 1U;
389 }
390
391 absIn0 = in0Lo;
392 in0Lo += productHiLo << 16U;
393 if (in0Lo < absIn0) {
394 absIn1++;
395 }
396
397 absIn0 = (((productLoHi >> 16U) + (productHiLo >> 16U)) + in0Hi * in1Hi) +
398 absIn1;
399 if (!((in0 == 0) || ((in1 == 0) || ((in0 > 0) == (in1 > 0))))) {
400 absIn0 = ~absIn0;
401 in0Lo = ~in0Lo;
402 in0Lo++;
403 if (in0Lo == 0U) {
404 absIn0++;
405 }
406 }
407
408 *ptrOutBitsHi = absIn0;
409 *ptrOutBitsLo = in0Lo;
410}
411
412int32_T mul_s32_s32_s32_sat(int32_T a, int32_T b)
413{
414 int32_T result;
415 uint32_T u32_chi;
416 uint32_T u32_clo;
417 mul_wide_s32(a, b, &u32_chi, &u32_clo);
418 if (((int32_T)u32_chi > 0) || ((u32_chi == 0U) && (u32_clo >= 2147483648U))) {
419 result = MAX_int32_T;
420 } else if (((int32_T)u32_chi < -1) || (((int32_T)u32_chi == -1) && (u32_clo <
421 2147483648U))) {
422 result = MIN_int32_T;
423 } else {
424 result = (int32_T)u32_clo;
425 }
426
427 return result;
428}
429
430/* Initial conditions for function-call system: '<S1>/Motor_XHHY' */
431void motor_Motor_XHHY_Init(RT_MODEL_motor * const motor_M, rtDW_Motor_XHHY_motor
432 *localDW, rtP_Motor_XHHY_motor *localP)
433{
434 uint32_T prevT_idx_0;
435 uint32_T prevT_idx_1;
436 prevT_idx_0 = motor_M->Timing.clockTick0;
437 prevT_idx_1 = motor_M->Timing.clockTickH0;
438 localDW->Motor_XHHY_PREV_T[0] = prevT_idx_0;
439 localDW->Motor_XHHY_PREV_T[1] = prevT_idx_1;
440
441 /* InitializeConditions for DiscreteIntegrator: '<S50>/Discrete-Time Integrator' */
442 localDW->DiscreteTimeIntegrator_DSTATE = localP->DiscreteTimeIntegrator_IC;
443
444 /* InitializeConditions for DiscreteIntegrator: '<S51>/Discrete-Time Integrator' */
445 localDW->DiscreteTimeIntegrator_DSTATE_i = localP->DiscreteTimeIntegrator_IC_l;
446 localDW->DiscreteTimeIntegrator_PrevRese = 2;
447
448 /* InitializeConditions for DiscreteIntegrator: '<S51>/Discrete-Time Integrator1' */
449 localDW->DiscreteTimeIntegrator1_DSTATE = localP->DiscreteTimeIntegrator1_IC;
450}
451
452/* Output and update for function-call system: '<S1>/Motor_XHHY' */
453void motor_Motor_XHHY(RT_MODEL_motor * const motor_M, int32_T rtu_JD_In, int32_T
454 rtu_Encode_Pos, int32_T rtu_Encode_Sp, uint8_T
455 rtu_Slect_port, int32_T rtu_SGWY_In, rtB_Motor_XHHY_motor *
456 localB, rtDW_Motor_XHHY_motor *localDW,
457 rtP_Motor_XHHY_motor *localP, real_T *rtd_Angle_extern,
458 real_T *rtd_Angle_internal, uint8_T *rtd_EN_extern,
459 int32_T *rtd_JD_XHHY, int32_T *rtd_JD_XHHY_HC, uint8_T
460 *rtd_KG_En, uint8_T *rtd_KG_JD, uint8_T *rtd_KG_clc,
461 real_T *rtd_P_KP, real_T *rtd_V_KI, real_T *rtd_V_KP,
462 real_T *rtd_chu_jd)
463{
464 uint32_T elapseT_H;
465 uint32_T prevT_idx_0;
466 uint32_T elapseTime_idx_0;
467 uint32_T prevT_idx_1;
468 int64m_T tmp;
469 int64m_T tmp_0;
470 int128m_T tmp_1;
471 real_T u0;
472 real_T u1;
473 real_T u2;
474 prevT_idx_0 = localDW->Motor_XHHY_PREV_T[0];
475 prevT_idx_1 = localDW->Motor_XHHY_PREV_T[1];
476 elapseTime_idx_0 = motor_M->Timing.clockTick0 - prevT_idx_0;
477 elapseT_H = motor_M->Timing.clockTickH0 - prevT_idx_1;
478 if (prevT_idx_0 > motor_M->Timing.clockTick0) {
479 elapseT_H--;
480 }
481
482 prevT_idx_0 = motor_M->Timing.clockTick0;
483 prevT_idx_1 = motor_M->Timing.clockTickH0;
484 localDW->Motor_XHHY_ELAPS_T[0] = elapseTime_idx_0;
485 localDW->Motor_XHHY_PREV_T[0] = prevT_idx_0;
486 localDW->Motor_XHHY_ELAPS_T[1] = elapseT_H;
487 localDW->Motor_XHHY_PREV_T[1] = prevT_idx_1;
488
489 /* DataStoreRead: '<S9>/chu_jd' */
490 localB->chu_jd = *rtd_chu_jd;
491
492 /* DataStoreRead: '<S9>/Data Store Read1' */
493 localB->DataStoreRead1 = *rtd_Angle_extern;
494
495 /* DataStoreRead: '<S9>/Data Store Read2' */
496 localB->DataStoreRead2 = *rtd_Angle_internal;
497
498 /* DataStoreRead: '<S9>/KG_En' */
499 localB->KG_En = *rtd_KG_En;
500
501 /* DataTypeConversion: '<S9>/Data Type Conversion1' */
502 localB->DataTypeConversion1 = rtu_Encode_Pos;
503
504 /* Gain: '<S9>/GXZ6' */
505 localB->GXZ6 = localP->GXZ6_Gain * localB->DataTypeConversion1;
506
507 /* Gain: '<S9>/GXZ1' */
508 localB->GXZ1 = localP->GXZ1_Gain * localB->GXZ6;
509
510 /* Gain: '<S9>/GXZ9' */
511 localB->GXZ9 = localP->GXZ9_Gain * localB->GXZ1;
512
513 /* Product: '<S9>/Product' */
514 localB->Product = (real_T)localB->KG_En * localB->GXZ9;
515
516 /* DataStoreRead: '<S9>/EN_extern' */
517 localB->EN_extern = *rtd_EN_extern;
518
519 /* DataTypeConversion: '<S9>/Data Type Conversion3' */
520 localB->DataTypeConversion3 = rtu_SGWY_In;
521
522 /* Gain: '<S9>/GXZ2' */
523 localB->GXZ2 = localP->GXZ2_Gain * localB->DataTypeConversion3;
524
525 /* Fcn: '<S57>/Fcn1' */
526 localB->Fcn1 = localB->GXZ2 + 265.5;
527
528 /* Fcn: '<S57>/Fcn2' */
529 localB->Fcn2 = (90490.25 - localB->Fcn1 * localB->Fcn1) / 20000.0;
530
531 /* Fcn: '<S57>/Fcn3' */
532 u0 = (7.0490249999999985 - localB->Fcn2 * localB->Fcn2) + 1.0;
533 if (u0 < 0.0) {
534 u0 = -sqrt(-u0);
535 } else {
536 u0 = sqrt(u0);
537 }
538
539 localB->Fcn3 = (2.655 - u0) / (localB->Fcn2 + 1.0);
540
541 /* End of Fcn: '<S57>/Fcn3' */
542
543 /* Fcn: '<S57>/Fcn6' */
544 localB->Fcn6 = 2.0 * atan(localB->Fcn3);
545
546 /* Fcn: '<S57>/Fcn4' */
547 localB->Fcn4 = 2.655 * cos(localB->Fcn6);
548
549 /* Fcn: '<S57>/Fcn10' */
550 localB->Fcn10 = localB->Fcn4 * localB->Fcn4 + 1.0;
551
552 /* Gain: '<S9>/GXZ7' */
553 localB->GXZ7 = localP->GXZ7_Gain * localB->GXZ6;
554
555 /* Fcn: '<S57>/Fcn5' */
556 localB->Fcn5 = localB->GXZ7 + 265.5;
557
558 /* Fcn: '<S57>/Fcn7' */
559 localB->Fcn7 = (90490.25 - localB->Fcn5 * localB->Fcn5) / 20000.0;
560
561 /* Fcn: '<S57>/Fcn8' */
562 localB->Fcn8 = localB->Fcn7 * localB->Fcn7;
563
564 /* Sum: '<S57>/Sum3' */
565 localB->Sum3 = localB->Fcn10 - localB->Fcn8;
566
567 /* Fcn: '<S57>/Fcn11' */
568 u0 = localB->Sum3;
569 if (u0 < 0.0) {
570 u0 = -sqrt(-u0);
571 } else {
572 u0 = sqrt(u0);
573 }
574
575 localB->Fcn11 = u0;
576
577 /* End of Fcn: '<S57>/Fcn11' */
578
579 /* Sum: '<S57>/Sum2' */
580 localB->Sum2 = localB->Fcn4 - localB->Fcn11;
581
582 /* Fcn: '<S57>/Fcn9' */
583 localB->Fcn9 = 1.0 / (localB->Fcn7 + 1.0);
584
585 /* Product: '<S57>/Product' */
586 localB->Product_p = localB->Sum2 * localB->Fcn9;
587
588 /* Fcn: '<S57>/Fcn12' */
589 localB->Fcn12 = 2.0 * atan(localB->Product_p);
590
591 /* Gain: '<S9>/GXH7' */
592 localB->GXH7 = localP->GXH7_Gain * localB->Fcn12;
593
594 /* Product: '<S9>/Product3' */
595 localB->Product3 = (real_T)localB->EN_extern * localB->GXH7;
596
597 /* Sum: '<S9>/Sum2' */
598 localB->Sum2_m = (localB->DataStoreRead2 + localB->Product) - localB->Product3;
599
600 /* Sum: '<S9>/Sum' */
601 localB->Sum = (localB->chu_jd + localB->DataStoreRead1) - localB->Sum2_m;
602
603 /* Saturate: '<S9>/XF_XHZY1' */
604 u0 = localB->Sum;
605 u1 = localP->XF_XHZY1_LowerSat;
606 u2 = localP->XF_XHZY1_UpperSat;
607 if (u0 > u2) {
608 localB->XF_XHZY1 = u2;
609 } else if (u0 < u1) {
610 localB->XF_XHZY1 = u1;
611 } else {
612 localB->XF_XHZY1 = u0;
613 }
614
615 /* End of Saturate: '<S9>/XF_XHZY1' */
616
617 /* Gain: '<S9>/CDB_XHHY' */
618 localB->CDB_XHHY = localP->CDB_XHHY_Gain * localB->XF_XHZY1;
619
620 /* DiscreteIntegrator: '<S50>/Discrete-Time Integrator' */
621 localB->DiscreteTimeIntegrator = localDW->DiscreteTimeIntegrator_DSTATE;
622
623 /* DataStoreRead: '<S50>/P_KP' */
624 localB->P_KP = *rtd_P_KP;
625
626 /* Product: '<S50>/Product' */
627 localB->Product_h = localB->CDB_XHHY * localB->P_KP;
628
629 /* Gain: '<S50>/ZP_Kp2' */
630 localB->ZP_Kp2 = localP->ZP_Kp2_Gain * localB->Product_h;
631
632 /* Gain: '<S50>/ZV_KD1' */
633 localB->ZV_KD1 = localP->ZV_KD1_Gain * localB->ZP_Kp2;
634
635 /* Sum: '<S50>/Sum' */
636 localB->Sum_p = localB->ZV_KD1 - localB->DiscreteTimeIntegrator;
637
638 /* Gain: '<S50>/ZP_KN' */
639 localB->ZP_KN = localP->ZP_KN_Gain * localB->Sum_p;
640
641 /* Gain: '<S50>/KD_KG' */
642 localB->KD_KG = localP->KD_KG_Gain * localB->ZP_KN;
643
644 /* Gain: '<S50>/ZP_Kp1' */
645 localB->ZP_Kp1 = localP->ZP_Kp1_Gain * localB->Product_h;
646
647 /* Sum: '<S50>/Sum1' */
648 localB->Sum1 = localB->ZP_Kp1 + localB->KD_KG;
649
650 /* DataStoreRead: '<S51>/KG_clc' */
651 localB->KG_clc = *rtd_KG_clc;
652
653 /* DiscreteIntegrator: '<S51>/Discrete-Time Integrator' */
654 if (((localB->KG_clc > 0) && (localDW->DiscreteTimeIntegrator_PrevRese <= 0)) ||
655 ((localB->KG_clc <= 0) && (localDW->DiscreteTimeIntegrator_PrevRese == 1)))
656 {
657 localDW->DiscreteTimeIntegrator_DSTATE_i =
658 localP->DiscreteTimeIntegrator_IC_l;
659 }
660
661 if (localDW->DiscreteTimeIntegrator_DSTATE_i >=
662 localP->DiscreteTimeIntegrator_UpperSat) {
663 localDW->DiscreteTimeIntegrator_DSTATE_i =
664 localP->DiscreteTimeIntegrator_UpperSat;
665 } else {
666 if (localDW->DiscreteTimeIntegrator_DSTATE_i <=
667 localP->DiscreteTimeIntegrator_LowerSat) {
668 localDW->DiscreteTimeIntegrator_DSTATE_i =
669 localP->DiscreteTimeIntegrator_LowerSat;
670 }
671 }
672
673 localB->DiscreteTimeIntegrator_b = localDW->DiscreteTimeIntegrator_DSTATE_i;
674
675 /* End of DiscreteIntegrator: '<S51>/Discrete-Time Integrator' */
676
677 /* DiscreteIntegrator: '<S51>/Discrete-Time Integrator1' */
678 localB->DiscreteTimeIntegrator1 = localDW->DiscreteTimeIntegrator1_DSTATE;
679
680 /* Saturate: '<S9>/Saturation' */
681 u0 = localB->Sum1;
682 u1 = localP->Saturation_LowerSat;
683 u2 = localP->Saturation_UpperSat;
684 if (u0 > u2) {
685 localB->Saturation = u2;
686 } else if (u0 < u1) {
687 localB->Saturation = u1;
688 } else {
689 localB->Saturation = u0;
690 }
691
692 /* End of Saturate: '<S9>/Saturation' */
693
694 /* Gain: '<S9>/GXZ4' */
695 prevT_idx_0 = (uint32_T)localP->GXZ4_Gain;
696 prevT_idx_1 = (uint32_T)rtu_Encode_Sp;
697 sMultiWordMul(&prevT_idx_0, 1, &prevT_idx_1, 1, &tmp.chunks[0U], 2);
698 localB->GXZ4 = tmp;
699
700 /* Gain: '<S9>/GXZ5' */
701 tmp = localP->GXZ5_Gain;
702 tmp_0 = localB->GXZ4;
703 sMultiWordMul(&tmp.chunks[0U], 2, &tmp_0.chunks[0U], 2, &tmp_1.chunks[0U], 4);
704 localB->GXZ5 = tmp_1;
705
706 /* Sum: '<S9>/Sum1' */
707 tmp_1 = localB->GXZ5;
708 localB->Sum1_d = localB->Saturation - sMultiWord2Double(&tmp_1.chunks[0U], 4,
709 0) * 2.0194839173657902E-28;
710
711 /* Gain: '<S51>/ZV_Kp' */
712 localB->ZV_Kp = localP->ZV_Kp_Gain * localB->Sum1_d;
713
714 /* Gain: '<S51>/ZV_Kp1' */
715 localB->ZV_Kp1 = localP->ZV_Kp1_Gain * localB->ZV_Kp;
716
717 /* Gain: '<S51>/ZV_KD' */
718 localB->ZV_KD = localP->ZV_KD_Gain * localB->ZV_Kp1;
719
720 /* Sum: '<S51>/Sum' */
721 localB->Sum_b = localB->ZV_KD - localB->DiscreteTimeIntegrator1;
722
723 /* Gain: '<S51>/ZV_KN' */
724 localB->ZV_KN = localP->ZV_KN_Gain * localB->Sum_b;
725
726 /* Saturate: '<S51>/Saturation' */
727 u0 = localB->ZV_KN;
728 u1 = localP->Saturation_LowerSat_l;
729 u2 = localP->Saturation_UpperSat_l;
730 if (u0 > u2) {
731 localB->Saturation_a = u2;
732 } else if (u0 < u1) {
733 localB->Saturation_a = u1;
734 } else {
735 localB->Saturation_a = u0;
736 }
737
738 /* End of Saturate: '<S51>/Saturation' */
739
740 /* Gain: '<S51>/KD_KG' */
741 localB->KD_KG_a = localP->KD_KG_Gain_m * localB->Saturation_a;
742
743 /* DataStoreRead: '<S51>/V_KP' */
744 localB->V_KP = *rtd_V_KP;
745
746 /* Product: '<S51>/Product' */
747 localB->Product_e = localB->ZV_Kp * localB->V_KP;
748
749 /* DataStoreRead: '<S51>/V_KI' */
750 localB->V_KI = *rtd_V_KI;
751
752 /* Product: '<S51>/Product1' */
753 localB->Product1 = localB->ZV_Kp * localB->V_KI;
754
755 /* Signum: '<S51>/Sign' */
756 u0 = localB->Saturation;
757 if (u0 < 0.0) {
758 localB->Sign = -1.0;
759 } else if (u0 > 0.0) {
760 localB->Sign = 1.0;
761 } else {
762 localB->Sign = u0;
763 }
764
765 /* End of Signum: '<S51>/Sign' */
766
767 /* Gain: '<S51>/ZV1_Kp1' */
768 localB->ZV1_Kp1 = localP->ZV1_Kp1_Gain * localB->Saturation;
769
770 /* Gain: '<S51>/ZV1_Kp2' */
771 localB->ZV1_Kp2 = localP->ZV1_Kp2_Gain * localB->Sign;
772
773 /* Sum: '<S51>/Sum2' */
774 localB->Sum2_n = (localB->DiscreteTimeIntegrator_b + localB->Product_e) +
775 localB->KD_KG_a;
776
777 /* Sum: '<S51>/Sum1' */
778 localB->Sum1_du = (localB->ZV1_Kp1 + localB->ZV1_Kp2) + localB->Sum2_n;
779
780 /* Gain: '<S9>/KP_JD1' */
781 localB->KP_JD1 = localP->KP_JD1_Gain * localB->GXH7;
782
783 /* Gain: '<S9>/KP_e' */
784 localB->KP_e = localP->KP_e_Gain * localB->Sum;
785
786 /* Saturate: '<S9>/ZXF_PID' */
787 u0 = localB->Sum1_du;
788 u1 = localP->ZXF_PID_LowerSat;
789 u2 = localP->ZXF_PID_UpperSat;
790 if (u0 > u2) {
791 localB->ZXF_PID = u2;
792 } else if (u0 < u1) {
793 localB->ZXF_PID = u1;
794 } else {
795 localB->ZXF_PID = u0;
796 }
797
798 /* End of Saturate: '<S9>/ZXF_PID' */
799
800 /* Sum: '<S9>/Subtract' incorporates:
801 * Constant: '<S9>/Con_ZPWM'
802 */
803 localB->Subtract = localB->ZXF_PID + localP->Con_ZPWM_Value;
804
805 /* Saturate: '<S9>/ZXF_PWM' */
806 u0 = localB->Subtract;
807 u1 = localP->ZXF_PWM_LowerSat;
808 u2 = localP->ZXF_PWM_UpperSat;
809 if (u0 > u2) {
810 localB->ZXF_PWM = u2;
811 } else if (u0 < u1) {
812 localB->ZXF_PWM = u1;
813 } else {
814 localB->ZXF_PWM = u0;
815 }
816
817 /* End of Saturate: '<S9>/ZXF_PWM' */
818
819 /* If: '<S9>/If' */
820 if (rtu_Slect_port == 5) {
821 /* Outputs for IfAction SubSystem: '<S9>/If Action Subsystem3' incorporates:
822 * ActionPort: '<S54>/Action Port'
823 */
824 /* DataStoreWrite: '<S54>/Data Store Write' */
825 *rtd_JD_XHHY_HC = rtu_JD_In;
826
827 /* DataStoreWrite: '<S54>/Data Store Write1' */
828 *rtd_JD_XHHY = rtu_JD_In;
829
830 /* End of Outputs for SubSystem: '<S9>/If Action Subsystem3' */
831 } else if (rtu_Slect_port == 4) {
832 /* Outputs for IfAction SubSystem: '<S9>/If Action Subsystem1' incorporates:
833 * ActionPort: '<S52>/Action Port'
834 */
835 /* DataStoreRead: '<S52>/Data Store Read' */
836 localB->DataStoreRead_c = *rtd_JD_XHHY_HC;
837
838 /* DataStoreWrite: '<S52>/Data Store Write1' */
839 *rtd_JD_XHHY = localB->DataStoreRead_c;
840
841 /* End of Outputs for SubSystem: '<S9>/If Action Subsystem1' */
842 } else {
843 /* Outputs for IfAction SubSystem: '<S9>/If Action Subsystem2' incorporates:
844 * ActionPort: '<S53>/Action Port'
845 */
846 /* DataStoreWrite: '<S53>/Data Store Write' */
847 *rtd_JD_XHHY_HC = rtu_JD_In;
848
849 /* DataStoreWrite: '<S53>/Data Store Write1' */
850 *rtd_JD_XHHY = rtu_JD_In;
851
852 /* End of Outputs for SubSystem: '<S9>/If Action Subsystem2' */
853 }
854
855 /* End of If: '<S9>/If' */
856
857 /* DataStoreRead: '<S9>/KG_JD1' */
858 localB->KG_JD1 = *rtd_EN_extern;
859
860 /* DataStoreRead: '<S9>/KG_JD' */
861 localB->KG_JD = *rtd_KG_JD;
862
863 /* DataTypeConversion: '<S9>/Data Type Conversion4' */
864 localB->DataTypeConversion4 = localB->KG_JD;
865
866 /* DataStoreRead: '<S9>/Data Store Read' */
867 localB->DataStoreRead = *rtd_JD_XHHY;
868
869 /* DataTypeConversion: '<S9>/Data Type Conversion2' */
870 localB->DataTypeConversion2 = localB->DataStoreRead;
871
872 /* Gain: '<S9>/GXZ3' */
873 localB->GXZ3 = localP->GXZ3_Gain * localB->DataTypeConversion2;
874
875 /* Saturate: '<S9>/XF_XHZY' */
876 u0 = localB->GXZ3;
877 u1 = localP->XF_XHZY_LowerSat;
878 u2 = localP->XF_XHZY_UpperSat;
879 if (u0 > u2) {
880 localB->XF_XHZY = u2;
881 } else if (u0 < u1) {
882 localB->XF_XHZY = u1;
883 } else {
884 localB->XF_XHZY = u0;
885 }
886
887 /* End of Saturate: '<S9>/XF_XHZY' */
888
889 /* Product: '<S9>/Product1' */
890 localB->Product1_l = localB->DataTypeConversion4 * localB->XF_XHZY;
891
892 /* If: '<S9>/If1' */
893 if (localB->KG_JD1 > 0) {
894 /* Outputs for IfAction SubSystem: '<S9>/Switch Case Action Subsystem' incorporates:
895 * ActionPort: '<S55>/Action Port'
896 */
897 /* DataStoreWrite: '<S55>/Data Store Write' */
898 *rtd_Angle_extern = localB->Product1_l;
899
900 /* DataStoreWrite: '<S55>/Data Store Write1' incorporates:
901 * Constant: '<S55>/Constant'
902 */
903 *rtd_Angle_internal = localP->Constant_Value;
904
905 /* End of Outputs for SubSystem: '<S9>/Switch Case Action Subsystem' */
906 } else {
907 /* Outputs for IfAction SubSystem: '<S9>/Switch Case Action Subsystem1' incorporates:
908 * ActionPort: '<S56>/Action Port'
909 */
910 /* DataStoreWrite: '<S56>/Data Store Write1' */
911 *rtd_Angle_internal = localB->Product1_l;
912
913 /* DataStoreWrite: '<S56>/Data Store Write' incorporates:
914 * Constant: '<S56>/Constant'
915 */
916 *rtd_Angle_extern = localP->Constant_Value_a;
917
918 /* End of Outputs for SubSystem: '<S9>/Switch Case Action Subsystem1' */
919 }
920
921 /* End of If: '<S9>/If1' */
922
923 /* Update for DiscreteIntegrator: '<S50>/Discrete-Time Integrator' */
924 localDW->DiscreteTimeIntegrator_DSTATE +=
925 localP->DiscreteTimeIntegrator_gainval * (real_T)localDW->
926 Motor_XHHY_ELAPS_T[0] * localB->ZP_KN;
927
928 /* Update for DiscreteIntegrator: '<S51>/Discrete-Time Integrator' */
929 localDW->DiscreteTimeIntegrator_DSTATE_i +=
930 localP->DiscreteTimeIntegrator_gainva_j * (real_T)
931 localDW->Motor_XHHY_ELAPS_T[0] * localB->Product1;
932 if (localDW->DiscreteTimeIntegrator_DSTATE_i >=
933 localP->DiscreteTimeIntegrator_UpperSat) {
934 localDW->DiscreteTimeIntegrator_DSTATE_i =
935 localP->DiscreteTimeIntegrator_UpperSat;
936 } else {
937 if (localDW->DiscreteTimeIntegrator_DSTATE_i <=
938 localP->DiscreteTimeIntegrator_LowerSat) {
939 localDW->DiscreteTimeIntegrator_DSTATE_i =
940 localP->DiscreteTimeIntegrator_LowerSat;
941 }
942 }
943
944 localDW->DiscreteTimeIntegrator_PrevRese = (int8_T)(localB->KG_clc > 0);
945
946 /* End of Update for DiscreteIntegrator: '<S51>/Discrete-Time Integrator' */
947
948 /* Update for DiscreteIntegrator: '<S51>/Discrete-Time Integrator1' */
949 localDW->DiscreteTimeIntegrator1_DSTATE +=
950 localP->DiscreteTimeIntegrator1_gainval * (real_T)
951 localDW->Motor_XHHY_ELAPS_T[0] * localB->ZV_KN;
952 if (localDW->DiscreteTimeIntegrator1_DSTATE >=
953 localP->DiscreteTimeIntegrator1_UpperSa) {
954 localDW->DiscreteTimeIntegrator1_DSTATE =
955 localP->DiscreteTimeIntegrator1_UpperSa;
956 } else {
957 if (localDW->DiscreteTimeIntegrator1_DSTATE <=
958 localP->DiscreteTimeIntegrator1_LowerSa) {
959 localDW->DiscreteTimeIntegrator1_DSTATE =
960 localP->DiscreteTimeIntegrator1_LowerSa;
961 }
962 }
963
964 /* End of Update for DiscreteIntegrator: '<S51>/Discrete-Time Integrator1' */
965}
966
967/* Initial conditions for function-call system: '<S1>/Motor_HYDG' */
968void motor_Motor_HYDG_Init(RT_MODEL_motor * const motor_M, rtDW_Motor_HYDG_motor
969 *localDW, rtP_Motor_HYDG_motor *localP)
970{
971 uint32_T prevT_idx_0;
972 uint32_T prevT_idx_1;
973 prevT_idx_0 = motor_M->Timing.clockTick0;
974 prevT_idx_1 = motor_M->Timing.clockTickH0;
975 localDW->Motor_HYDG_PREV_T[0] = prevT_idx_0;
976 localDW->Motor_HYDG_PREV_T[1] = prevT_idx_1;
977
978 /* InitializeConditions for DiscreteIntegrator: '<S35>/Discrete-Time Integrator' */
979 localDW->DiscreteTimeIntegrator_DSTATE = localP->DiscreteTimeIntegrator_IC;
980
981 /* InitializeConditions for DiscreteIntegrator: '<S36>/Discrete-Time Integrator' */
982 localDW->DiscreteTimeIntegrator_DSTATE_i = localP->DiscreteTimeIntegrator_IC_b;
983 localDW->DiscreteTimeIntegrator_PrevRese = 2;
984
985 /* InitializeConditions for DiscreteIntegrator: '<S36>/Discrete-Time Integrator1' */
986 localDW->DiscreteTimeIntegrator1_DSTATE = localP->DiscreteTimeIntegrator1_IC;
987}
988
989/* Output and update for function-call system: '<S1>/Motor_HYDG' */
990void motor_Motor_HYDG(RT_MODEL_motor * const motor_M, int32_T rtu_JD_In, int32_T
991 rtu_Encode_Pos, uint8_T rtu_Slect_port, int32_T
992 rtu_Encode_Sp, rtB_Motor_HYDG_motor *localB,
993 rtDW_Motor_HYDG_motor *localDW, rtP_Motor_HYDG_motor
994 *localP, int32_T *rtd_JD_HYDG, int32_T *rtd_JD_HYDG_HC,
995 uint8_T *rtd_KG_JD, uint8_T *rtd_KG_clc, real_T
996 *rtd_chu_jd)
997{
998 int128m_T tmp;
999 int128m_T tmp_0;
1000 uint32_T elapseT_H;
1001 uint32_T prevT_idx_0;
1002 uint32_T elapseTime_idx_0;
1003 uint32_T prevT_idx_1;
1004 int64m_T tmp_1;
1005 int64m_T tmp_2;
1006 int128m_T tmp_3;
1007 int128m_T tmp_4;
1008 real_T u0;
1009 real_T u1;
1010 real_T u2;
1011 prevT_idx_0 = localDW->Motor_HYDG_PREV_T[0];
1012 prevT_idx_1 = localDW->Motor_HYDG_PREV_T[1];
1013 elapseTime_idx_0 = motor_M->Timing.clockTick0 - prevT_idx_0;
1014 elapseT_H = motor_M->Timing.clockTickH0 - prevT_idx_1;
1015 if (prevT_idx_0 > motor_M->Timing.clockTick0) {
1016 elapseT_H--;
1017 }
1018
1019 prevT_idx_0 = motor_M->Timing.clockTick0;
1020 prevT_idx_1 = motor_M->Timing.clockTickH0;
1021 localDW->Motor_HYDG_ELAPS_T[0] = elapseTime_idx_0;
1022 localDW->Motor_HYDG_PREV_T[0] = prevT_idx_0;
1023 localDW->Motor_HYDG_ELAPS_T[1] = elapseT_H;
1024 localDW->Motor_HYDG_PREV_T[1] = prevT_idx_1;
1025
1026 /* DataStoreRead: '<S7>/chu_jd' */
1027 localB->chu_jd = *rtd_chu_jd;
1028
1029 /* DataStoreRead: '<S7>/KG_JD' */
1030 localB->KG_JD = *rtd_KG_JD;
1031
1032 /* DataStoreRead: '<S7>/Data Store Read' */
1033 localB->DataStoreRead = *rtd_JD_HYDG;
1034
1035 /* Gain: '<S7>/GHDG10' */
1036 prevT_idx_0 = (uint32_T)localP->GHDG10_Gain;
1037 prevT_idx_1 = (uint32_T)localB->DataStoreRead;
1038 sMultiWordMul(&prevT_idx_0, 1, &prevT_idx_1, 1, &tmp_1.chunks[0U], 2);
1039 localB->GHDG10 = tmp_1;
1040
1041 /* Gain: '<S7>/GHDG1' */
1042 tmp_1 = localP->GHDG1_Gain;
1043 tmp_2 = localB->GHDG10;
1044 sMultiWordMul(&tmp_1.chunks[0U], 2, &tmp_2.chunks[0U], 2, &tmp_3.chunks[0U], 4);
1045 localB->GHDG1 = tmp_3;
1046
1047 /* Saturate: '<S7>/XF_XHZY' */
1048 tmp_3 = localB->GHDG1;
1049 tmp = localP->XF_XHZY_LowerSat;
1050 tmp_0 = localP->XF_XHZY_UpperSat;
1051 if (sMultiWordGt(&tmp_3.chunks[0U], &tmp_0.chunks[0U], 4)) {
1052 localB->XF_XHZY = tmp_0;
1053 } else if (sMultiWordLt(&tmp_3.chunks[0U], &tmp.chunks[0U], 4)) {
1054 localB->XF_XHZY = tmp;
1055 } else {
1056 localB->XF_XHZY = tmp_3;
1057 }
1058
1059 /* End of Saturate: '<S7>/XF_XHZY' */
1060
1061 /* Product: '<S7>/Product' */
1062 tmp_3 = localB->XF_XHZY;
1063 localB->Product = sMultiWord2Double(&tmp_3.chunks[0U], 4, 0) *
1064 1.9721522630525295E-31 * (real_T)localB->KG_JD;
1065
1066 /* DataTypeConversion: '<S7>/Data Type Conversion1' */
1067 localB->DataTypeConversion1 = rtu_Encode_Pos;
1068
1069 /* Gain: '<S7>/GHDG5' */
1070 localB->GHDG5 = localP->GHDG5_Gain * localB->DataTypeConversion1;
1071
1072 /* Gain: '<S40>/GHDG7' */
1073 localB->GHDG7 = localP->GHDG7_Gain * localB->GHDG5;
1074
1075 /* Gain: '<S40>/GHDG8' */
1076 localB->GHDG8 = localP->GHDG8_Gain * localB->GHDG7;
1077
1078 /* Fcn: '<S41>/Fcn7' */
1079 localB->Fcn7 = localB->GHDG8 + 0.2655;
1080
1081 /* Fcn: '<S41>/Cl1' */
1082 localB->Cl1 = (0.090490250000000008 - localB->Fcn7 * localB->Fcn7) /
1083 0.020000000000000004;
1084
1085 /* Fcn: '<S41>/Fcn' */
1086 u0 = (7.0490249999999985 - localB->Cl1 * localB->Cl1) + 1.0;
1087 if (u0 < 0.0) {
1088 u0 = -sqrt(-u0);
1089 } else {
1090 u0 = sqrt(u0);
1091 }
1092
1093 localB->Fcn = (2.655 - u0) / (localB->Cl1 + 1.0);
1094
1095 /* End of Fcn: '<S41>/Fcn' */
1096
1097 /* Fcn: '<S41>/Fcn4' */
1098 localB->Fcn4 = 2.0 * atan(localB->Fcn);
1099
1100 /* Gain: '<S40>/GHDG9' */
1101 localB->GHDG9 = localP->GHDG9_Gain * localB->Fcn4;
1102
1103 /* Gain: '<S7>/fk_dg' */
1104 localB->fk_dg = localP->fk_dg_Gain * localB->GHDG9;
1105
1106 /* Sum: '<S7>/Sum' */
1107 localB->Sum = (localB->chu_jd + localB->Product) - localB->fk_dg;
1108
1109 /* Saturate: '<S7>/JD_e(k)' */
1110 u0 = localB->Sum;
1111 u1 = localP->JD_ek_LowerSat;
1112 u2 = localP->JD_ek_UpperSat;
1113 if (u0 > u2) {
1114 localB->JD_ek = u2;
1115 } else if (u0 < u1) {
1116 localB->JD_ek = u1;
1117 } else {
1118 localB->JD_ek = u0;
1119 }
1120
1121 /* End of Saturate: '<S7>/JD_e(k)' */
1122
1123 /* Gain: '<S7>/CDB_XHHY' */
1124 localB->CDB_XHHY = localP->CDB_XHHY_Gain * localB->JD_ek;
1125
1126 /* DiscreteIntegrator: '<S35>/Discrete-Time Integrator' */
1127 localB->DiscreteTimeIntegrator = localDW->DiscreteTimeIntegrator_DSTATE;
1128
1129 /* Gain: '<S35>/ZP_Kp' */
1130 localB->ZP_Kp = localP->ZP_Kp_Gain * localB->CDB_XHHY;
1131
1132 /* Gain: '<S35>/ZP_Kp2' */
1133 localB->ZP_Kp2 = localP->ZP_Kp2_Gain * localB->ZP_Kp;
1134
1135 /* Gain: '<S35>/ZV_KD1' */
1136 localB->ZV_KD1 = localP->ZV_KD1_Gain * localB->ZP_Kp2;
1137
1138 /* Sum: '<S35>/Sum' */
1139 localB->Sum_g = localB->ZV_KD1 - localB->DiscreteTimeIntegrator;
1140
1141 /* Gain: '<S35>/ZP_KN' */
1142 localB->ZP_KN = localP->ZP_KN_Gain * localB->Sum_g;
1143
1144 /* Gain: '<S35>/KD_KG' */
1145 localB->KD_KG = localP->KD_KG_Gain * localB->ZP_KN;
1146
1147 /* Gain: '<S35>/ZP_Kp1' */
1148 localB->ZP_Kp1 = localP->ZP_Kp1_Gain * localB->ZP_Kp;
1149
1150 /* Sum: '<S35>/Sum1' */
1151 localB->Sum1 = localB->ZP_Kp1 + localB->KD_KG;
1152
1153 /* DataStoreRead: '<S36>/KG_clc' */
1154 localB->KG_clc = *rtd_KG_clc;
1155
1156 /* DiscreteIntegrator: '<S36>/Discrete-Time Integrator' */
1157 if (((localB->KG_clc > 0) && (localDW->DiscreteTimeIntegrator_PrevRese <= 0)) ||
1158 ((localB->KG_clc <= 0) && (localDW->DiscreteTimeIntegrator_PrevRese == 1)))
1159 {
1160 localDW->DiscreteTimeIntegrator_DSTATE_i =
1161 localP->DiscreteTimeIntegrator_IC_b;
1162 }
1163
1164 if (localDW->DiscreteTimeIntegrator_DSTATE_i >=
1165 localP->DiscreteTimeIntegrator_UpperSat) {
1166 localDW->DiscreteTimeIntegrator_DSTATE_i =
1167 localP->DiscreteTimeIntegrator_UpperSat;
1168 } else {
1169 if (localDW->DiscreteTimeIntegrator_DSTATE_i <=
1170 localP->DiscreteTimeIntegrator_LowerSat) {
1171 localDW->DiscreteTimeIntegrator_DSTATE_i =
1172 localP->DiscreteTimeIntegrator_LowerSat;
1173 }
1174 }
1175
1176 localB->DiscreteTimeIntegrator_o = localDW->DiscreteTimeIntegrator_DSTATE_i;
1177
1178 /* End of DiscreteIntegrator: '<S36>/Discrete-Time Integrator' */
1179
1180 /* DiscreteIntegrator: '<S36>/Discrete-Time Integrator1' */
1181 localB->DiscreteTimeIntegrator1 = localDW->DiscreteTimeIntegrator1_DSTATE;
1182
1183 /* Saturate: '<S7>/Saturation' */
1184 u0 = localB->Sum1;
1185 u1 = localP->Saturation_LowerSat;
1186 u2 = localP->Saturation_UpperSat;
1187 if (u0 > u2) {
1188 localB->Saturation = u2;
1189 } else if (u0 < u1) {
1190 localB->Saturation = u1;
1191 } else {
1192 localB->Saturation = u0;
1193 }
1194
1195 /* End of Saturate: '<S7>/Saturation' */
1196
1197 /* Gain: '<S7>/GXZ4' incorporates:
1198 * Gain: '<S7>/GHDG1'
1199 */
1200 prevT_idx_0 = (uint32_T)localP->GXZ4_Gain;
1201 prevT_idx_1 = (uint32_T)rtu_Encode_Sp;
1202 sMultiWordMul(&prevT_idx_0, 1, &prevT_idx_1, 1, &tmp_1.chunks[0U], 2);
1203 localB->GXZ4 = tmp_1;
1204
1205 /* Gain: '<S7>/GXZ5' */
1206 tmp_2 = localP->GXZ5_Gain;
1207 tmp_1 = localB->GXZ4;
1208 sMultiWordMul(&tmp_2.chunks[0U], 2, &tmp_1.chunks[0U], 2, &tmp_4.chunks[0U], 4);
1209 localB->GXZ5 = tmp_4;
1210
1211 /* Sum: '<S7>/Sum2' */
1212 tmp_3 = localB->GXZ5;
1213 localB->Sum2 = localB->Saturation - sMultiWord2Double(&tmp_3.chunks[0U], 4, 0)
1214 * 4.0389678347315804E-28;
1215
1216 /* Gain: '<S36>/ZV_Kp' */
1217 localB->ZV_Kp = localP->ZV_Kp_Gain * localB->Sum2;
1218
1219 /* Gain: '<S36>/ZV_Kp1' */
1220 localB->ZV_Kp1 = localP->ZV_Kp1_Gain * localB->ZV_Kp;
1221
1222 /* Gain: '<S36>/ZV_KD' */
1223 localB->ZV_KD = localP->ZV_KD_Gain * localB->ZV_Kp1;
1224
1225 /* Sum: '<S36>/Sum' */
1226 localB->Sum_n = localB->ZV_KD - localB->DiscreteTimeIntegrator1;
1227
1228 /* Gain: '<S36>/ZV_KN' */
1229 localB->ZV_KN = localP->ZV_KN_Gain * localB->Sum_n;
1230
1231 /* Saturate: '<S36>/Saturation' */
1232 u0 = localB->ZV_KN;
1233 u1 = localP->Saturation_LowerSat_i;
1234 u2 = localP->Saturation_UpperSat_m;
1235 if (u0 > u2) {
1236 localB->Saturation_b = u2;
1237 } else if (u0 < u1) {
1238 localB->Saturation_b = u1;
1239 } else {
1240 localB->Saturation_b = u0;
1241 }
1242
1243 /* End of Saturate: '<S36>/Saturation' */
1244
1245 /* Gain: '<S36>/KD_KG' */
1246 localB->KD_KG_l = localP->KD_KG_Gain_l * localB->Saturation_b;
1247
1248 /* Signum: '<S36>/Sign' */
1249 u0 = localB->Saturation;
1250 if (u0 < 0.0) {
1251 localB->Sign = -1.0;
1252 } else if (u0 > 0.0) {
1253 localB->Sign = 1.0;
1254 } else {
1255 localB->Sign = u0;
1256 }
1257
1258 /* End of Signum: '<S36>/Sign' */
1259
1260 /* Gain: '<S36>/ZV1_Kp1' */
1261 localB->ZV1_Kp1 = localP->ZV1_Kp1_Gain * localB->Saturation;
1262
1263 /* Gain: '<S36>/ZV1_Kp2' */
1264 localB->ZV1_Kp2 = localP->ZV1_Kp2_Gain * localB->Sign;
1265
1266 /* Gain: '<S36>/ZV_Ki1' */
1267 localB->ZV_Ki1 = localP->ZV_Ki1_Gain * localB->DiscreteTimeIntegrator_o;
1268
1269 /* Gain: '<S36>/ZV_Kpt' */
1270 localB->ZV_Kpt = localP->ZV_Kpt_Gain * localB->ZV_Kp;
1271
1272 /* Sum: '<S36>/Sum2' */
1273 localB->Sum2_e = (localB->ZV_Ki1 + localB->ZV_Kpt) + localB->KD_KG_l;
1274
1275 /* Sum: '<S36>/Sum1' */
1276 localB->Sum1_g = (localB->ZV1_Kp1 + localB->ZV1_Kp2) + localB->Sum2_e;
1277
1278 /* Saturate: '<S7>/XF_PID' */
1279 u0 = localB->Sum1_g;
1280 u1 = localP->XF_PID_LowerSat;
1281 u2 = localP->XF_PID_UpperSat;
1282 if (u0 > u2) {
1283 localB->XF_PID = u2;
1284 } else if (u0 < u1) {
1285 localB->XF_PID = u1;
1286 } else {
1287 localB->XF_PID = u0;
1288 }
1289
1290 /* End of Saturate: '<S7>/XF_PID' */
1291
1292 /* Sum: '<S7>/Sum3' incorporates:
1293 * Constant: '<S7>/Constant'
1294 */
1295 localB->Sum3 = localB->XF_PID + localP->Constant_Value;
1296
1297 /* Saturate: '<S7>/XF_PWM' */
1298 u0 = localB->Sum3;
1299 u1 = localP->XF_PWM_LowerSat;
1300 u2 = localP->XF_PWM_UpperSat;
1301 if (u0 > u2) {
1302 localB->XF_PWM = u2;
1303 } else if (u0 < u1) {
1304 localB->XF_PWM = u1;
1305 } else {
1306 localB->XF_PWM = u0;
1307 }
1308
1309 /* End of Saturate: '<S7>/XF_PWM' */
1310
1311 /* DataTypeConversion: '<S7>/Data Type Conversion3' */
1312 u0 = fmod(floor(localB->XF_PWM), 65536.0);
1313 localB->DataTypeConversion3 = (uint16_T)(u0 < 0.0 ? (int32_T)(uint16_T)
1314 -(int16_T)(uint16_T)-u0 : (int32_T)(uint16_T)u0);
1315
1316 /* Gain: '<S7>/KP_JD' */
1317 localB->KP_JD = localP->KP_JD_Gain * localB->fk_dg;
1318
1319 /* Gain: '<S7>/KP_e' */
1320 localB->KP_e = localP->KP_e_Gain * localB->Sum;
1321
1322 /* If: '<S7>/If' */
1323 if (rtu_Slect_port == 5) {
1324 /* Outputs for IfAction SubSystem: '<S7>/If Action Subsystem' incorporates:
1325 * ActionPort: '<S37>/Action Port'
1326 */
1327 /* DataStoreWrite: '<S37>/Data Store Write' */
1328 *rtd_JD_HYDG_HC = rtu_JD_In;
1329
1330 /* DataStoreWrite: '<S37>/Data Store Write1' */
1331 *rtd_JD_HYDG = rtu_JD_In;
1332
1333 /* End of Outputs for SubSystem: '<S7>/If Action Subsystem' */
1334 } else if (rtu_Slect_port == 4) {
1335 /* Outputs for IfAction SubSystem: '<S7>/If Action Subsystem1' incorporates:
1336 * ActionPort: '<S38>/Action Port'
1337 */
1338 /* DataStoreRead: '<S38>/Data Store Read' */
1339 localB->DataStoreRead_e = *rtd_JD_HYDG_HC;
1340
1341 /* DataStoreWrite: '<S38>/Data Store Write1' */
1342 *rtd_JD_HYDG = localB->DataStoreRead_e;
1343
1344 /* End of Outputs for SubSystem: '<S7>/If Action Subsystem1' */
1345 } else {
1346 /* Outputs for IfAction SubSystem: '<S7>/If Action Subsystem2' incorporates:
1347 * ActionPort: '<S39>/Action Port'
1348 */
1349 /* DataStoreWrite: '<S39>/Data Store Write' */
1350 *rtd_JD_HYDG_HC = rtu_JD_In;
1351
1352 /* DataStoreWrite: '<S39>/Data Store Write1' */
1353 *rtd_JD_HYDG = rtu_JD_In;
1354
1355 /* End of Outputs for SubSystem: '<S7>/If Action Subsystem2' */
1356 }
1357
1358 /* End of If: '<S7>/If' */
1359
1360 /* Update for DiscreteIntegrator: '<S35>/Discrete-Time Integrator' */
1361 localDW->DiscreteTimeIntegrator_DSTATE +=
1362 localP->DiscreteTimeIntegrator_gainval * (real_T)localDW->
1363 Motor_HYDG_ELAPS_T[0] * localB->ZP_KN;
1364
1365 /* Update for DiscreteIntegrator: '<S36>/Discrete-Time Integrator' */
1366 localDW->DiscreteTimeIntegrator_DSTATE_i +=
1367 localP->DiscreteTimeIntegrator_gainva_p * (real_T)
1368 localDW->Motor_HYDG_ELAPS_T[0] * localB->ZV_Kp;
1369 if (localDW->DiscreteTimeIntegrator_DSTATE_i >=
1370 localP->DiscreteTimeIntegrator_UpperSat) {
1371 localDW->DiscreteTimeIntegrator_DSTATE_i =
1372 localP->DiscreteTimeIntegrator_UpperSat;
1373 } else {
1374 if (localDW->DiscreteTimeIntegrator_DSTATE_i <=
1375 localP->DiscreteTimeIntegrator_LowerSat) {
1376 localDW->DiscreteTimeIntegrator_DSTATE_i =
1377 localP->DiscreteTimeIntegrator_LowerSat;
1378 }
1379 }
1380
1381 localDW->DiscreteTimeIntegrator_PrevRese = (int8_T)(localB->KG_clc > 0);
1382
1383 /* End of Update for DiscreteIntegrator: '<S36>/Discrete-Time Integrator' */
1384
1385 /* Update for DiscreteIntegrator: '<S36>/Discrete-Time Integrator1' */
1386 localDW->DiscreteTimeIntegrator1_DSTATE +=
1387 localP->DiscreteTimeIntegrator1_gainval * (real_T)
1388 localDW->Motor_HYDG_ELAPS_T[0] * localB->ZV_KN;
1389}
1390
1391/* Initial conditions for function-call system: '<S1>/Motor_XHZY' */
1392void motor_Motor_XHZY_Init(RT_MODEL_motor * const motor_M, rtDW_Motor_XHZY_motor
1393 *localDW, rtP_Motor_XHZY_motor *localP)
1394{
1395 uint32_T prevT_idx_0;
1396 uint32_T prevT_idx_1;
1397 prevT_idx_0 = motor_M->Timing.clockTick0;
1398 prevT_idx_1 = motor_M->Timing.clockTickH0;
1399 localDW->Motor_XHZY_PREV_T[0] = prevT_idx_0;
1400 localDW->Motor_XHZY_PREV_T[1] = prevT_idx_1;
1401
1402 /* InitializeConditions for DiscreteIntegrator: '<S58>/Discrete-Time Integrator' */
1403 localDW->DiscreteTimeIntegrator_DSTATE = localP->DiscreteTimeIntegrator_IC;
1404
1405 /* InitializeConditions for DiscreteIntegrator: '<S59>/Discrete-Time Integrator' */
1406 localDW->DiscreteTimeIntegrator_DSTATE_c = localP->DiscreteTimeIntegrator_IC_l;
1407 localDW->DiscreteTimeIntegrator_PrevRese = 2;
1408
1409 /* InitializeConditions for DiscreteIntegrator: '<S59>/Discrete-Time Integrator1' */
1410 localDW->DiscreteTimeIntegrator1_DSTATE = localP->DiscreteTimeIntegrator1_IC;
1411}
1412
1413/* Output and update for function-call system: '<S1>/Motor_XHZY' */
1414void motor_Motor_XHZY(RT_MODEL_motor * const motor_M, int32_T rtu_JD_In, int32_T
1415 rtu_Encode_Pos, int32_T rtu_YJ_In, int32_T rtu_Encode_Sp,
1416 uint8_T rtu_Slect_port, rtB_Motor_XHZY_motor *localB,
1417 rtDW_Motor_XHZY_motor *localDW, rtP_Motor_XHZY_motor
1418 *localP, real_T *rtd_Angle_extern, real_T
1419 *rtd_Angle_internal, uint8_T *rtd_EN_extern, uint8_T
1420 *rtd_Forword, int32_T *rtd_JD_XHZY, int32_T
1421 *rtd_JD_XHZY_HC, uint8_T *rtd_KG_En, uint8_T *rtd_KG_JD,
1422 uint8_T *rtd_KG_YJ, uint8_T *rtd_KG_clc, real_T *rtd_P_KP,
1423 real_T *rtd_V_KI, real_T *rtd_V_KP, int32_T *rtd_YJ_XHZY,
1424 int32_T *rtd_YJ_XHZY_HC, real_T *rtd_chu_jd)
1425{
1426 uint32_T elapseT_H;
1427 uint32_T prevT_idx_0;
1428 uint32_T elapseTime_idx_0;
1429 uint32_T prevT_idx_1;
1430 int64m_T tmp;
1431 int64m_T tmp_0;
1432 int128m_T tmp_1;
1433 real_T u1;
1434 real_T u2;
1435 real_T u;
1436 prevT_idx_0 = localDW->Motor_XHZY_PREV_T[0];
1437 prevT_idx_1 = localDW->Motor_XHZY_PREV_T[1];
1438 elapseTime_idx_0 = motor_M->Timing.clockTick0 - prevT_idx_0;
1439 elapseT_H = motor_M->Timing.clockTickH0 - prevT_idx_1;
1440 if (prevT_idx_0 > motor_M->Timing.clockTick0) {
1441 elapseT_H--;
1442 }
1443
1444 prevT_idx_0 = motor_M->Timing.clockTick0;
1445 prevT_idx_1 = motor_M->Timing.clockTickH0;
1446 localDW->Motor_XHZY_ELAPS_T[0] = elapseTime_idx_0;
1447 localDW->Motor_XHZY_PREV_T[0] = prevT_idx_0;
1448 localDW->Motor_XHZY_ELAPS_T[1] = elapseT_H;
1449 localDW->Motor_XHZY_PREV_T[1] = prevT_idx_1;
1450
1451 /* DataStoreRead: '<S10>/chu_jd' */
1452 localB->chu_jd = *rtd_chu_jd;
1453
1454 /* DataStoreRead: '<S10>/KG_YJ' */
1455 localB->KG_YJ = *rtd_KG_YJ;
1456
1457 /* DataStoreRead: '<S10>/Data Store Read1' */
1458 localB->DataStoreRead1 = *rtd_YJ_XHZY;
1459
1460 /* DataTypeConversion: '<S10>/Data Type Conversion3' */
1461 localB->DataTypeConversion3 = localB->DataStoreRead1;
1462
1463 /* Gain: '<S10>/GXZ2' */
1464 localB->GXZ2 = localP->GXZ2_Gain * localB->DataTypeConversion3;
1465
1466 /* Saturate: '<S10>/XF_YJ' */
1467 u = localB->GXZ2;
1468 u1 = localP->XF_YJ_LowerSat;
1469 u2 = localP->XF_YJ_UpperSat;
1470 if (u > u2) {
1471 localB->XF_YJ = u2;
1472 } else if (u < u1) {
1473 localB->XF_YJ = u1;
1474 } else {
1475 localB->XF_YJ = u;
1476 }
1477
1478 /* End of Saturate: '<S10>/XF_YJ' */
1479
1480 /* Product: '<S10>/Product2' */
1481 localB->Product2 = (real_T)localB->KG_YJ * localB->XF_YJ;
1482
1483 /* DataStoreRead: '<S10>/Data Store Read2' */
1484 localB->DataStoreRead2 = *rtd_Angle_extern;
1485
1486 /* DataStoreRead: '<S10>/Data Store Read3' */
1487 localB->DataStoreRead3 = *rtd_Angle_internal;
1488
1489 /* DataStoreRead: '<S10>/KG_En' */
1490 localB->KG_En = *rtd_KG_En;
1491
1492 /* DataTypeConversion: '<S10>/Data Type Conversion1' */
1493 localB->DataTypeConversion1 = rtu_Encode_Pos;
1494
1495 /* Gain: '<S10>/GXZ6' */
1496 localB->GXZ6 = localP->GXZ6_Gain * localB->DataTypeConversion1;
1497
1498 /* Gain: '<S10>/GXZ1' */
1499 localB->GXZ1 = localP->GXZ1_Gain * localB->GXZ6;
1500
1501 /* Gain: '<S10>/GXZ9' */
1502 localB->GXZ9 = localP->GXZ9_Gain * localB->GXZ1;
1503
1504 /* Product: '<S10>/Product' */
1505 localB->Product = (real_T)localB->KG_En * localB->GXZ9;
1506
1507 /* DataStoreRead: '<S10>/EN_extern' */
1508 localB->EN_extern = *rtd_EN_extern;
1509
1510 /* Gain: '<S10>/GXZ7' */
1511 localB->GXZ7 = localP->GXZ7_Gain * localB->GXZ6;
1512
1513 /* Fcn: '<S65>/Fcn1' */
1514 localB->Fcn1 = localB->GXZ7 + 265.5;
1515
1516 /* Fcn: '<S65>/Fcn2' */
1517 localB->Fcn2 = (90490.25 - localB->Fcn1 * localB->Fcn1) / 20000.0;
1518
1519 /* Fcn: '<S65>/Fcn3' */
1520 u = (7.0490249999999985 - localB->Fcn2 * localB->Fcn2) + 1.0;
1521 if (u < 0.0) {
1522 u = -sqrt(-u);
1523 } else {
1524 u = sqrt(u);
1525 }
1526
1527 localB->Fcn3 = (2.655 - u) / (localB->Fcn2 + 1.0);
1528
1529 /* End of Fcn: '<S65>/Fcn3' */
1530
1531 /* Fcn: '<S65>/Fcn6' */
1532 localB->Fcn6 = 2.0 * atan(localB->Fcn3);
1533
1534 /* Gain: '<S10>/GXZ8' */
1535 localB->GXZ8 = localP->GXZ8_Gain * localB->Fcn6;
1536
1537 /* Product: '<S10>/Product3' */
1538 localB->Product3 = (real_T)localB->EN_extern * localB->GXZ8;
1539
1540 /* Sum: '<S10>/Sum2' */
1541 localB->Sum2 = (localB->DataStoreRead3 + localB->Product) - localB->Product3;
1542
1543 /* Sum: '<S10>/Sum' */
1544 localB->Sum = ((localB->chu_jd + localB->Product2) + localB->DataStoreRead2) -
1545 localB->Sum2;
1546
1547 /* Saturate: '<S10>/JD_e(k)' */
1548 u = localB->Sum;
1549 u1 = localP->JD_ek_LowerSat;
1550 u2 = localP->JD_ek_UpperSat;
1551 if (u > u2) {
1552 localB->JD_ek = u2;
1553 } else if (u < u1) {
1554 localB->JD_ek = u1;
1555 } else {
1556 localB->JD_ek = u;
1557 }
1558
1559 /* End of Saturate: '<S10>/JD_e(k)' */
1560
1561 /* Gain: '<S10>/CDB_XHHY' */
1562 localB->CDB_XHHY = localP->CDB_XHHY_Gain * localB->JD_ek;
1563
1564 /* DiscreteIntegrator: '<S58>/Discrete-Time Integrator' */
1565 localB->DiscreteTimeIntegrator = localDW->DiscreteTimeIntegrator_DSTATE;
1566
1567 /* DataStoreRead: '<S58>/P_KP' */
1568 localB->P_KP = *rtd_P_KP;
1569
1570 /* Product: '<S58>/Product' */
1571 localB->Product_k = localB->CDB_XHHY * localB->P_KP;
1572
1573 /* Gain: '<S58>/ZP_Kp2' */
1574 localB->ZP_Kp2 = localP->ZP_Kp2_Gain * localB->Product_k;
1575
1576 /* Gain: '<S58>/ZV_KD1' */
1577 localB->ZV_KD1 = localP->ZV_KD1_Gain * localB->ZP_Kp2;
1578
1579 /* Sum: '<S58>/Sum' */
1580 localB->Sum_e = localB->ZV_KD1 - localB->DiscreteTimeIntegrator;
1581
1582 /* Gain: '<S58>/ZP_KN' */
1583 localB->ZP_KN = localP->ZP_KN_Gain * localB->Sum_e;
1584
1585 /* Gain: '<S58>/KD_KG' */
1586 localB->KD_KG = localP->KD_KG_Gain * localB->ZP_KN;
1587
1588 /* Gain: '<S58>/ZP_Kp1' */
1589 localB->ZP_Kp1 = localP->ZP_Kp1_Gain * localB->Product_k;
1590
1591 /* Sum: '<S58>/Sum1' */
1592 localB->Sum1 = localB->ZP_Kp1 + localB->KD_KG;
1593
1594 /* DataStoreRead: '<S59>/KG_clc' */
1595 localB->KG_clc = *rtd_KG_clc;
1596
1597 /* DiscreteIntegrator: '<S59>/Discrete-Time Integrator' */
1598 if (((localB->KG_clc > 0) && (localDW->DiscreteTimeIntegrator_PrevRese <= 0)) ||
1599 ((localB->KG_clc <= 0) && (localDW->DiscreteTimeIntegrator_PrevRese == 1)))
1600 {
1601 localDW->DiscreteTimeIntegrator_DSTATE_c =
1602 localP->DiscreteTimeIntegrator_IC_l;
1603 }
1604
1605 if (localDW->DiscreteTimeIntegrator_DSTATE_c >=
1606 localP->DiscreteTimeIntegrator_UpperSat) {
1607 localDW->DiscreteTimeIntegrator_DSTATE_c =
1608 localP->DiscreteTimeIntegrator_UpperSat;
1609 } else {
1610 if (localDW->DiscreteTimeIntegrator_DSTATE_c <=
1611 localP->DiscreteTimeIntegrator_LowerSat) {
1612 localDW->DiscreteTimeIntegrator_DSTATE_c =
1613 localP->DiscreteTimeIntegrator_LowerSat;
1614 }
1615 }
1616
1617 localB->DiscreteTimeIntegrator_b = localDW->DiscreteTimeIntegrator_DSTATE_c;
1618
1619 /* End of DiscreteIntegrator: '<S59>/Discrete-Time Integrator' */
1620
1621 /* DiscreteIntegrator: '<S59>/Discrete-Time Integrator1' */
1622 localB->DiscreteTimeIntegrator1 = localDW->DiscreteTimeIntegrator1_DSTATE;
1623
1624 /* Saturate: '<S10>/Saturation' */
1625 u = localB->Sum1;
1626 u1 = localP->Saturation_LowerSat;
1627 u2 = localP->Saturation_UpperSat;
1628 if (u > u2) {
1629 localB->Saturation = u2;
1630 } else if (u < u1) {
1631 localB->Saturation = u1;
1632 } else {
1633 localB->Saturation = u;
1634 }
1635
1636 /* End of Saturate: '<S10>/Saturation' */
1637
1638 /* Gain: '<S10>/GXZ4' */
1639 prevT_idx_0 = (uint32_T)localP->GXZ4_Gain;
1640 prevT_idx_1 = (uint32_T)rtu_Encode_Sp;
1641 sMultiWordMul(&prevT_idx_0, 1, &prevT_idx_1, 1, &tmp.chunks[0U], 2);
1642 localB->GXZ4 = tmp;
1643
1644 /* Gain: '<S10>/GXZ5' */
1645 tmp = localP->GXZ5_Gain;
1646 tmp_0 = localB->GXZ4;
1647 sMultiWordMul(&tmp.chunks[0U], 2, &tmp_0.chunks[0U], 2, &tmp_1.chunks[0U], 4);
1648 localB->GXZ5 = tmp_1;
1649
1650 /* Sum: '<S10>/Sum1' */
1651 tmp_1 = localB->GXZ5;
1652 localB->Sum1_n = localB->Saturation - sMultiWord2Double(&tmp_1.chunks[0U], 4,
1653 0) * 2.0194839173657902E-28;
1654
1655 /* Gain: '<S59>/ZV_Kp' */
1656 localB->ZV_Kp = localP->ZV_Kp_Gain * localB->Sum1_n;
1657
1658 /* Gain: '<S59>/ZV_Kp1' */
1659 localB->ZV_Kp1 = localP->ZV_Kp1_Gain * localB->ZV_Kp;
1660
1661 /* Gain: '<S59>/ZV_KD' */
1662 localB->ZV_KD = localP->ZV_KD_Gain * localB->ZV_Kp1;
1663
1664 /* Sum: '<S59>/Sum' */
1665 localB->Sum_i = localB->ZV_KD - localB->DiscreteTimeIntegrator1;
1666
1667 /* Gain: '<S59>/ZV_KN' */
1668 localB->ZV_KN = localP->ZV_KN_Gain * localB->Sum_i;
1669
1670 /* Saturate: '<S59>/Saturation' */
1671 u = localB->ZV_KN;
1672 u1 = localP->Saturation_LowerSat_p;
1673 u2 = localP->Saturation_UpperSat_f;
1674 if (u > u2) {
1675 localB->Saturation_p = u2;
1676 } else if (u < u1) {
1677 localB->Saturation_p = u1;
1678 } else {
1679 localB->Saturation_p = u;
1680 }
1681
1682 /* End of Saturate: '<S59>/Saturation' */
1683
1684 /* Gain: '<S59>/KD_KG' */
1685 localB->KD_KG_a = localP->KD_KG_Gain_c * localB->Saturation_p;
1686
1687 /* DataStoreRead: '<S59>/V_KP' */
1688 localB->V_KP = *rtd_V_KP;
1689
1690 /* Product: '<S59>/Product' */
1691 localB->Product_p = localB->ZV_Kp * localB->V_KP;
1692
1693 /* DataStoreRead: '<S59>/V_KI' */
1694 localB->V_KI = *rtd_V_KI;
1695
1696 /* Product: '<S59>/Product1' */
1697 localB->Product1 = localB->ZV_Kp * localB->V_KI;
1698
1699 /* DataStoreRead: '<S10>/Forword' */
1700 localB->Forword = *rtd_Forword;
1701
1702 /* Saturate: '<S10>/Saturation1' */
1703 u = localB->Saturation;
1704 u1 = localP->Saturation1_LowerSat;
1705 u2 = localP->Saturation1_UpperSat;
1706 if (u > u2) {
1707 localB->Saturation1 = u2;
1708 } else if (u < u1) {
1709 localB->Saturation1 = u1;
1710 } else {
1711 localB->Saturation1 = u;
1712 }
1713
1714 /* End of Saturate: '<S10>/Saturation1' */
1715
1716 /* Product: '<S10>/Product4' */
1717 localB->Product4 = (real_T)localB->Forword * localB->Saturation1;
1718
1719 /* Signum: '<S59>/Sign' */
1720 u = localB->Product4;
1721 if (u < 0.0) {
1722 localB->Sign = -1.0;
1723 } else if (u > 0.0) {
1724 localB->Sign = 1.0;
1725 } else {
1726 localB->Sign = u;
1727 }
1728
1729 /* End of Signum: '<S59>/Sign' */
1730
1731 /* Gain: '<S59>/ZV1_Kp1' */
1732 localB->ZV1_Kp1 = localP->ZV1_Kp1_Gain * localB->Product4;
1733
1734 /* Gain: '<S59>/ZV1_Kp2' */
1735 localB->ZV1_Kp2 = localP->ZV1_Kp2_Gain * localB->Sign;
1736
1737 /* Sum: '<S59>/Sum2' */
1738 localB->Sum2_h = (localB->DiscreteTimeIntegrator_b + localB->Product_p) +
1739 localB->KD_KG_a;
1740
1741 /* Sum: '<S59>/Sum1' */
1742 localB->Sum1_i = (localB->ZV1_Kp1 + localB->ZV1_Kp2) + localB->Sum2_h;
1743
1744 /* Gain: '<S10>/KP_JD1' */
1745 localB->KP_JD1 = localP->KP_JD1_Gain * localB->GXZ7;
1746
1747 /* DataTypeConversion: '<S10>/Data Type Conversion4' */
1748 u = fmod(floor(localB->KP_JD1), 4.294967296E+9);
1749 localB->DataTypeConversion4 = u < 0.0 ? -(int32_T)(uint32_T)-u : (int32_T)
1750 (uint32_T)u;
1751
1752 /* Gain: '<S10>/KP_JD' */
1753 localB->KP_JD = localP->KP_JD_Gain * localB->GXZ8;
1754
1755 /* Gain: '<S10>/KP_e' */
1756 localB->KP_e = localP->KP_e_Gain * localB->Sum;
1757
1758 /* Saturate: '<S10>/ZXF_PID' */
1759 u = localB->Sum1_i;
1760 u1 = localP->ZXF_PID_LowerSat;
1761 u2 = localP->ZXF_PID_UpperSat;
1762 if (u > u2) {
1763 localB->ZXF_PID = u2;
1764 } else if (u < u1) {
1765 localB->ZXF_PID = u1;
1766 } else {
1767 localB->ZXF_PID = u;
1768 }
1769
1770 /* End of Saturate: '<S10>/ZXF_PID' */
1771
1772 /* Sum: '<S10>/Subtract' incorporates:
1773 * Constant: '<S10>/Con_ZPWM'
1774 */
1775 localB->Subtract = localB->ZXF_PID + localP->Con_ZPWM_Value;
1776
1777 /* Saturate: '<S10>/ZXF_PWM' */
1778 u = localB->Subtract;
1779 u1 = localP->ZXF_PWM_LowerSat;
1780 u2 = localP->ZXF_PWM_UpperSat;
1781 if (u > u2) {
1782 localB->ZXF_PWM = u2;
1783 } else if (u < u1) {
1784 localB->ZXF_PWM = u1;
1785 } else {
1786 localB->ZXF_PWM = u;
1787 }
1788
1789 /* End of Saturate: '<S10>/ZXF_PWM' */
1790
1791 /* If: '<S10>/If' */
1792 if (rtu_Slect_port == 5) {
1793 /* Outputs for IfAction SubSystem: '<S10>/If Action Subsystem3' incorporates:
1794 * ActionPort: '<S62>/Action Port'
1795 */
1796 /* DataStoreWrite: '<S62>/Data Store Write' */
1797 *rtd_JD_XHZY_HC = rtu_JD_In;
1798
1799 /* DataStoreWrite: '<S62>/Data Store Write1' */
1800 *rtd_JD_XHZY = rtu_JD_In;
1801
1802 /* DataStoreWrite: '<S62>/Data Store Write2' */
1803 *rtd_YJ_XHZY_HC = rtu_YJ_In;
1804
1805 /* DataStoreWrite: '<S62>/Data Store Write3' */
1806 *rtd_YJ_XHZY = rtu_YJ_In;
1807
1808 /* End of Outputs for SubSystem: '<S10>/If Action Subsystem3' */
1809 } else if (rtu_Slect_port == 4) {
1810 /* Outputs for IfAction SubSystem: '<S10>/If Action Subsystem1' incorporates:
1811 * ActionPort: '<S60>/Action Port'
1812 */
1813 /* DataStoreRead: '<S60>/Data Store Read' */
1814 localB->DataStoreRead_m = *rtd_JD_XHZY_HC;
1815
1816 /* DataStoreWrite: '<S60>/Data Store Write1' */
1817 *rtd_JD_XHZY = localB->DataStoreRead_m;
1818
1819 /* DataStoreRead: '<S60>/Data Store Read1' */
1820 localB->DataStoreRead1_d = *rtd_YJ_XHZY_HC;
1821
1822 /* DataStoreWrite: '<S60>/Data Store Write2' */
1823 *rtd_YJ_XHZY = localB->DataStoreRead1_d;
1824
1825 /* End of Outputs for SubSystem: '<S10>/If Action Subsystem1' */
1826 } else {
1827 /* Outputs for IfAction SubSystem: '<S10>/If Action Subsystem2' incorporates:
1828 * ActionPort: '<S61>/Action Port'
1829 */
1830 /* DataStoreWrite: '<S61>/Data Store Write' */
1831 *rtd_JD_XHZY_HC = rtu_JD_In;
1832
1833 /* DataStoreWrite: '<S61>/Data Store Write1' */
1834 *rtd_JD_XHZY = rtu_JD_In;
1835
1836 /* DataStoreWrite: '<S61>/Data Store Write2' */
1837 *rtd_YJ_XHZY_HC = rtu_YJ_In;
1838
1839 /* DataStoreWrite: '<S61>/Data Store Write3' */
1840 *rtd_YJ_XHZY = rtu_YJ_In;
1841
1842 /* End of Outputs for SubSystem: '<S10>/If Action Subsystem2' */
1843 }
1844
1845 /* End of If: '<S10>/If' */
1846
1847 /* DataStoreRead: '<S10>/KG_JD1' */
1848 localB->KG_JD1 = *rtd_EN_extern;
1849
1850 /* DataStoreRead: '<S10>/KG_JD' */
1851 localB->KG_JD = *rtd_KG_JD;
1852
1853 /* DataStoreRead: '<S10>/Data Store Read' */
1854 localB->DataStoreRead = *rtd_JD_XHZY;
1855
1856 /* DataTypeConversion: '<S10>/Data Type Conversion2' */
1857 localB->DataTypeConversion2 = localB->DataStoreRead;
1858
1859 /* Gain: '<S10>/GXZ3' */
1860 localB->GXZ3 = localP->GXZ3_Gain * localB->DataTypeConversion2;
1861
1862 /* Gain: '<S10>/GXZ10' */
1863 localB->GXZ10 = localP->GXZ10_Gain * localB->GXZ3;
1864
1865 /* Saturate: '<S10>/XF_XHZY' */
1866 u = localB->GXZ10;
1867 u1 = localP->XF_XHZY_LowerSat;
1868 u2 = localP->XF_XHZY_UpperSat;
1869 if (u > u2) {
1870 localB->XF_XHZY = u2;
1871 } else if (u < u1) {
1872 localB->XF_XHZY = u1;
1873 } else {
1874 localB->XF_XHZY = u;
1875 }
1876
1877 /* End of Saturate: '<S10>/XF_XHZY' */
1878
1879 /* Product: '<S10>/Product1' */
1880 localB->Product1_a = (real_T)localB->KG_JD * localB->XF_XHZY;
1881
1882 /* If: '<S10>/If1' */
1883 if (localB->KG_JD1 > 0) {
1884 /* Outputs for IfAction SubSystem: '<S10>/Switch Case Action Subsystem' incorporates:
1885 * ActionPort: '<S63>/Action Port'
1886 */
1887 /* DataStoreWrite: '<S63>/Data Store Write' */
1888 *rtd_Angle_extern = localB->Product1_a;
1889
1890 /* DataStoreWrite: '<S63>/Data Store Write1' incorporates:
1891 * Constant: '<S63>/Constant'
1892 */
1893 *rtd_Angle_internal = localP->Constant_Value;
1894
1895 /* End of Outputs for SubSystem: '<S10>/Switch Case Action Subsystem' */
1896 } else {
1897 /* Outputs for IfAction SubSystem: '<S10>/Switch Case Action Subsystem1' incorporates:
1898 * ActionPort: '<S64>/Action Port'
1899 */
1900 /* DataStoreWrite: '<S64>/Data Store Write1' */
1901 *rtd_Angle_internal = localB->Product1_a;
1902
1903 /* DataStoreWrite: '<S64>/Data Store Write' incorporates:
1904 * Constant: '<S64>/Constant'
1905 */
1906 *rtd_Angle_extern = localP->Constant_Value_k;
1907
1908 /* End of Outputs for SubSystem: '<S10>/Switch Case Action Subsystem1' */
1909 }
1910
1911 /* End of If: '<S10>/If1' */
1912
1913 /* Update for DiscreteIntegrator: '<S58>/Discrete-Time Integrator' */
1914 localDW->DiscreteTimeIntegrator_DSTATE +=
1915 localP->DiscreteTimeIntegrator_gainval * (real_T)localDW->
1916 Motor_XHZY_ELAPS_T[0] * localB->ZP_KN;
1917
1918 /* Update for DiscreteIntegrator: '<S59>/Discrete-Time Integrator' */
1919 localDW->DiscreteTimeIntegrator_DSTATE_c +=
1920 localP->DiscreteTimeIntegrator_gainva_m * (real_T)
1921 localDW->Motor_XHZY_ELAPS_T[0] * localB->Product1;
1922 if (localDW->DiscreteTimeIntegrator_DSTATE_c >=
1923 localP->DiscreteTimeIntegrator_UpperSat) {
1924 localDW->DiscreteTimeIntegrator_DSTATE_c =
1925 localP->DiscreteTimeIntegrator_UpperSat;
1926 } else {
1927 if (localDW->DiscreteTimeIntegrator_DSTATE_c <=
1928 localP->DiscreteTimeIntegrator_LowerSat) {
1929 localDW->DiscreteTimeIntegrator_DSTATE_c =
1930 localP->DiscreteTimeIntegrator_LowerSat;
1931 }
1932 }
1933
1934 localDW->DiscreteTimeIntegrator_PrevRese = (int8_T)(localB->KG_clc > 0);
1935
1936 /* End of Update for DiscreteIntegrator: '<S59>/Discrete-Time Integrator' */
1937
1938 /* Update for DiscreteIntegrator: '<S59>/Discrete-Time Integrator1' */
1939 localDW->DiscreteTimeIntegrator1_DSTATE +=
1940 localP->DiscreteTimeIntegrator1_gainval * (real_T)
1941 localDW->Motor_XHZY_ELAPS_T[0] * localB->ZV_KN;
1942}
1943
1944/*
1945 * Initial conditions for action system:
1946 * '<S23>/If Action Subsystem'
1947 * '<S24>/If Action Subsystem'
1948 * '<S25>/If Action Subsystem'
1949 */
1950void motor_IfActionSubsystem_Init(rtDW_IfActionSubsystem_motor_n *localDW)
1951{
1952 /* InitializeConditions for DiscretePulseGenerator: '<S26>/Pulse Generator' */
1953 localDW->clockTickCounter = 0;
1954}
1955
1956/*
1957 * Start for action system:
1958 * '<S23>/If Action Subsystem'
1959 * '<S24>/If Action Subsystem'
1960 * '<S25>/If Action Subsystem'
1961 */
1962void motor_IfActionSubsystem_Start(rtDW_IfActionSubsystem_motor_n *localDW)
1963{
1964 /* InitializeConditions for IfAction SubSystem: '<S23>/If Action Subsystem' */
1965 motor_IfActionSubsystem_Init(localDW);
1966
1967 /* End of InitializeConditions for SubSystem: '<S23>/If Action Subsystem' */
1968}
1969
1970/*
1971 * Output and update for action system:
1972 * '<S23>/If Action Subsystem'
1973 * '<S24>/If Action Subsystem'
1974 * '<S25>/If Action Subsystem'
1975 */
1976void motor_IfActionSubsystem(rtB_IfActionSubsystem_motor_k *localB,
1977 rtDW_IfActionSubsystem_motor_n *localDW, rtP_IfActionSubsystem_motor_m *localP,
1978 boolean_T *rtd_PC1, boolean_T *rtd_PC2)
1979{
1980 /* DiscretePulseGenerator: '<S26>/Pulse Generator' */
1981 localB->PulseGenerator = (localDW->clockTickCounter <
1982 localP->PulseGenerator_Duty) && (localDW->clockTickCounter >= 0) ?
1983 localP->PulseGenerator_Amp : 0.0;
1984 if (localDW->clockTickCounter >= localP->PulseGenerator_Period - 1.0) {
1985 localDW->clockTickCounter = 0;
1986 } else {
1987 localDW->clockTickCounter++;
1988 }
1989
1990 /* End of DiscretePulseGenerator: '<S26>/Pulse Generator' */
1991
1992 /* DataTypeConversion: '<S26>/Data Type Conversion' */
1993 localB->DataTypeConversion = (localB->PulseGenerator != 0.0);
1994
1995 /* DataStoreWrite: '<S26>/Data Store Write' */
1996 *rtd_PC1 = localB->DataTypeConversion;
1997
1998 /* DataStoreWrite: '<S26>/Data Store Write1' */
1999 *rtd_PC2 = localB->DataTypeConversion;
2000}
2001
2002/*
2003 * Initial conditions for action system:
2004 * '<S23>/If Action Subsystem1'
2005 * '<S24>/If Action Subsystem1'
2006 * '<S25>/If Action Subsystem1'
2007 */
2008void motor_IfActionSubsystem1_Init(rtDW_IfActionSubsystem1_motor *localDW)
2009{
2010 /* InitializeConditions for DiscretePulseGenerator: '<S27>/Pulse Generator' */
2011 localDW->clockTickCounter = 0;
2012}
2013
2014/*
2015 * Start for action system:
2016 * '<S23>/If Action Subsystem1'
2017 * '<S24>/If Action Subsystem1'
2018 * '<S25>/If Action Subsystem1'
2019 */
2020void motor_IfActionSubsystem1_Start(rtDW_IfActionSubsystem1_motor *localDW)
2021{
2022 /* InitializeConditions for IfAction SubSystem: '<S23>/If Action Subsystem1' */
2023 motor_IfActionSubsystem1_Init(localDW);
2024
2025 /* End of InitializeConditions for SubSystem: '<S23>/If Action Subsystem1' */
2026}
2027
2028/*
2029 * Output and update for action system:
2030 * '<S23>/If Action Subsystem1'
2031 * '<S24>/If Action Subsystem1'
2032 * '<S25>/If Action Subsystem1'
2033 */
2034void motor_IfActionSubsystem1(rtB_IfActionSubsystem1_motor *localB,
2035 rtDW_IfActionSubsystem1_motor *localDW, rtP_IfActionSubsystem1_motor *localP,
2036 boolean_T *rtd_PC1, boolean_T *rtd_PC2)
2037{
2038 /* DataStoreWrite: '<S27>/Data Store Write1' incorporates:
2039 * Constant: '<S27>/Constant'
2040 */
2041 *rtd_PC2 = localP->Constant_Value;
2042
2043 /* DiscretePulseGenerator: '<S27>/Pulse Generator' */
2044 localB->PulseGenerator = (localDW->clockTickCounter <
2045 localP->PulseGenerator_Duty) && (localDW->clockTickCounter >= 0) ?
2046 localP->PulseGenerator_Amp : 0.0;
2047 if (localDW->clockTickCounter >= localP->PulseGenerator_Period - 1.0) {
2048 localDW->clockTickCounter = 0;
2049 } else {
2050 localDW->clockTickCounter++;
2051 }
2052
2053 /* End of DiscretePulseGenerator: '<S27>/Pulse Generator' */
2054
2055 /* DataTypeConversion: '<S27>/Data Type Conversion' */
2056 localB->DataTypeConversion = (localB->PulseGenerator != 0.0);
2057
2058 /* DataStoreWrite: '<S27>/Data Store Write' */
2059 *rtd_PC1 = localB->DataTypeConversion;
2060}
2061
2062/*
2063 * Start for action system:
2064 * '<S5>/If Action Subsystem'
2065 * '<S5>/If Action Subsystem2'
2066 */
2067void motor_IfActionSubsystem_p_Start(rtDW_IfActionSubsystem_motor *localDW)
2068{
2069 /* Start for IfAction SubSystem: '<S23>/If Action Subsystem' */
2070 motor_IfActionSubsystem_Start(&localDW->IfActionSubsystem);
2071
2072 /* End of Start for SubSystem: '<S23>/If Action Subsystem' */
2073
2074 /* Start for IfAction SubSystem: '<S23>/If Action Subsystem1' */
2075 motor_IfActionSubsystem1_Start(&localDW->IfActionSubsystem1);
2076
2077 /* End of Start for SubSystem: '<S23>/If Action Subsystem1' */
2078}
2079
2080/*
2081 * Output and update for action system:
2082 * '<S5>/If Action Subsystem'
2083 * '<S5>/If Action Subsystem2'
2084 */
2085void motor_IfActionSubsystem_n(real_T rtu_In1, rtB_IfActionSubsystem_motor
2086 *localB, rtDW_IfActionSubsystem_motor *localDW, rtP_IfActionSubsystem_motor
2087 *localP, boolean_T *rtd_PC1, boolean_T *rtd_PC2)
2088{
2089 /* If: '<S23>/If' */
2090 if (rtu_In1 == 2.0) {
2091 /* Outputs for IfAction SubSystem: '<S23>/If Action Subsystem' incorporates:
2092 * ActionPort: '<S26>/Action Port'
2093 */
2094 motor_IfActionSubsystem(&localB->IfActionSubsystem,
2095 &localDW->IfActionSubsystem, (rtP_IfActionSubsystem_motor_m *)
2096 &localP->IfActionSubsystem, rtd_PC1, rtd_PC2);
2097
2098 /* End of Outputs for SubSystem: '<S23>/If Action Subsystem' */
2099 } else if (rtu_In1 == 3.0) {
2100 /* Outputs for IfAction SubSystem: '<S23>/If Action Subsystem1' incorporates:
2101 * ActionPort: '<S27>/Action Port'
2102 */
2103 motor_IfActionSubsystem1(&localB->IfActionSubsystem1,
2104 &localDW->IfActionSubsystem1, (rtP_IfActionSubsystem1_motor *)
2105 &localP->IfActionSubsystem1, rtd_PC1, rtd_PC2);
2106
2107 /* End of Outputs for SubSystem: '<S23>/If Action Subsystem1' */
2108 } else {
2109 /* Outputs for IfAction SubSystem: '<S23>/If Action Subsystem2' incorporates:
2110 * ActionPort: '<S28>/Action Port'
2111 */
2112 /* DataStoreWrite: '<S28>/Data Store Write' incorporates:
2113 * Constant: '<S28>/Constant'
2114 */
2115 *rtd_PC1 = localP->Constant_Value;
2116
2117 /* DataStoreWrite: '<S28>/Data Store Write1' incorporates:
2118 * Constant: '<S28>/Constant1'
2119 */
2120 *rtd_PC2 = localP->Constant1_Value;
2121
2122 /* End of Outputs for SubSystem: '<S23>/If Action Subsystem2' */
2123 }
2124
2125 /* End of If: '<S23>/If' */
2126}
2127
2128/* Initial conditions for function-call system: '<S1>/Motor_HYDG1' */
2129void motor_Motor_HYDG1_Init(RT_MODEL_motor * const motor_M,
2130 rtDW_Motor_HYDG1_motor *localDW, rtP_Motor_HYDG1_motor *localP)
2131{
2132 uint32_T prevT_idx_0;
2133 uint32_T prevT_idx_1;
2134 prevT_idx_0 = motor_M->Timing.clockTick0;
2135 prevT_idx_1 = motor_M->Timing.clockTickH0;
2136 localDW->Motor_HYDG1_PREV_T[0] = prevT_idx_0;
2137 localDW->Motor_HYDG1_PREV_T[1] = prevT_idx_1;
2138
2139 /* InitializeConditions for DiscreteIntegrator: '<S42>/Discrete-Time Integrator' */
2140 localDW->DiscreteTimeIntegrator_DSTATE = localP->DiscreteTimeIntegrator_IC;
2141
2142 /* InitializeConditions for DiscreteIntegrator: '<S43>/Discrete-Time Integrator' */
2143 localDW->DiscreteTimeIntegrator_DSTATE_a = localP->DiscreteTimeIntegrator_IC_g;
2144 localDW->DiscreteTimeIntegrator_PrevRese = 2;
2145
2146 /* InitializeConditions for DiscreteIntegrator: '<S43>/Discrete-Time Integrator1' */
2147 localDW->DiscreteTimeIntegrator1_DSTATE = localP->DiscreteTimeIntegrator1_IC;
2148
2149 /* InitializeConditions for UnitDelay: '<S8>/Unit Delay' */
2150 localDW->UnitDelay_DSTATE = localP->UnitDelay_InitialCondition;
2151}
2152
2153/* Output and update for function-call system: '<S1>/Motor_HYDG1' */
2154void motor_Motor_HYDG1(RT_MODEL_motor * const motor_M, int32_T rtu_JD_In,
2155 int32_T rtu_Encode_Pos, uint8_T rtu_Slect_port, rtB_Motor_HYDG1_motor *localB,
2156 rtDW_Motor_HYDG1_motor *localDW, rtP_Motor_HYDG1_motor *localP, int32_T
2157 *rtd_JD_HYDG, int32_T *rtd_JD_HYDG_HC, uint8_T *rtd_KG_JD, uint8_T *rtd_KG_clc,
2158 real_T *rtd_P_KP, real_T *rtd_Saturation_limit_speed, real_T *rtd_V_KI, real_T
2159 *rtd_V_KP, real_T *rtd_chu_jd)
2160{
2161 int128m_T tmp;
2162 int128m_T tmp_0;
2163 uint32_T elapseT_H;
2164 uint32_T prevT_idx_0;
2165 uint32_T elapseTime_idx_0;
2166 uint32_T prevT_idx_1;
2167 int64m_T tmp_1;
2168 int64m_T tmp_2;
2169 int128m_T tmp_3;
2170 real_T u0;
2171 real_T u1;
2172 real_T u2;
2173 prevT_idx_0 = localDW->Motor_HYDG1_PREV_T[0];
2174 prevT_idx_1 = localDW->Motor_HYDG1_PREV_T[1];
2175 elapseTime_idx_0 = motor_M->Timing.clockTick0 - prevT_idx_0;
2176 elapseT_H = motor_M->Timing.clockTickH0 - prevT_idx_1;
2177 if (prevT_idx_0 > motor_M->Timing.clockTick0) {
2178 elapseT_H--;
2179 }
2180
2181 prevT_idx_0 = motor_M->Timing.clockTick0;
2182 prevT_idx_1 = motor_M->Timing.clockTickH0;
2183 localDW->Motor_HYDG1_ELAPS_T[0] = elapseTime_idx_0;
2184 localDW->Motor_HYDG1_PREV_T[0] = prevT_idx_0;
2185 localDW->Motor_HYDG1_ELAPS_T[1] = elapseT_H;
2186 localDW->Motor_HYDG1_PREV_T[1] = prevT_idx_1;
2187
2188 /* DiscreteIntegrator: '<S42>/Discrete-Time Integrator' */
2189 localB->DiscreteTimeIntegrator = localDW->DiscreteTimeIntegrator_DSTATE;
2190
2191 /* DataStoreRead: '<S8>/chu_jd' */
2192 localB->chu_jd = *rtd_chu_jd;
2193
2194 /* DataStoreRead: '<S8>/KG_JD' */
2195 localB->KG_JD = *rtd_KG_JD;
2196
2197 /* DataStoreRead: '<S8>/Data Store Read' */
2198 localB->DataStoreRead = *rtd_JD_HYDG;
2199
2200 /* Gain: '<S8>/GHDG10' */
2201 prevT_idx_0 = (uint32_T)localP->GHDG10_Gain;
2202 prevT_idx_1 = (uint32_T)localB->DataStoreRead;
2203 sMultiWordMul(&prevT_idx_0, 1, &prevT_idx_1, 1, &tmp_1.chunks[0U], 2);
2204 localB->GHDG10 = tmp_1;
2205
2206 /* Gain: '<S8>/GHDG1' */
2207 tmp_1 = localP->GHDG1_Gain;
2208 tmp_2 = localB->GHDG10;
2209 sMultiWordMul(&tmp_1.chunks[0U], 2, &tmp_2.chunks[0U], 2, &tmp_3.chunks[0U], 4);
2210 localB->GHDG1 = tmp_3;
2211
2212 /* Saturate: '<S8>/XF_XHZY' */
2213 tmp_3 = localB->GHDG1;
2214 tmp = localP->XF_XHZY_LowerSat;
2215 tmp_0 = localP->XF_XHZY_UpperSat;
2216 if (sMultiWordGt(&tmp_3.chunks[0U], &tmp_0.chunks[0U], 4)) {
2217 localB->XF_XHZY = tmp_0;
2218 } else if (sMultiWordLt(&tmp_3.chunks[0U], &tmp.chunks[0U], 4)) {
2219 localB->XF_XHZY = tmp;
2220 } else {
2221 localB->XF_XHZY = tmp_3;
2222 }
2223
2224 /* End of Saturate: '<S8>/XF_XHZY' */
2225
2226 /* Product: '<S8>/Product' */
2227 tmp_3 = localB->XF_XHZY;
2228 localB->Product = sMultiWord2Double(&tmp_3.chunks[0U], 4, 0) *
2229 9.8607613152626476E-32 * (real_T)localB->KG_JD;
2230
2231 /* Sum: '<S8>/Sum4' */
2232 localB->Sum4 = localB->chu_jd + localB->Product;
2233
2234 /* Gain: '<S8>/GHDG2' */
2235 localB->GHDG2 = localP->GHDG2_Gain * localB->Sum4;
2236
2237 /* Fcn: '<S47>/Fcn' */
2238 localB->Fcn = (1.0 - cos(localB->GHDG2)) * 0.1;
2239
2240 /* Fcn: '<S47>/Fcn1' */
2241 localB->Fcn1 = localB->Fcn * localB->Fcn;
2242
2243 /* Fcn: '<S47>/Fcn2' */
2244 localB->Fcn2 = 0.2655 - 0.1 * sin(localB->GHDG2);
2245
2246 /* Fcn: '<S47>/Fcn3' */
2247 localB->Fcn3 = localB->Fcn2 * localB->Fcn2;
2248
2249 /* Sum: '<S47>/Sum1' */
2250 localB->Sum1 = localB->Fcn1 + localB->Fcn3;
2251
2252 /* Fcn: '<S47>/Fcn4' */
2253 u0 = localB->Sum1;
2254 if (u0 < 0.0) {
2255 u0 = -sqrt(-u0);
2256 } else {
2257 u0 = sqrt(u0);
2258 }
2259
2260 localB->Fcn4 = u0;
2261
2262 /* End of Fcn: '<S47>/Fcn4' */
2263
2264 /* Fcn: '<S47>/Fcn15' */
2265 localB->Fcn15 = localB->Fcn4 - 0.2655;
2266
2267 /* Gain: '<S8>/GHDG3' */
2268 localB->GHDG3 = localP->GHDG3_Gain * localB->Fcn15;
2269
2270 /* Gain: '<S8>/GHDG4' */
2271 localB->GHDG4 = localP->GHDG4_Gain * localB->GHDG3;
2272
2273 /* DataTypeConversion: '<S8>/Data Type Conversion1' */
2274 localB->DataTypeConversion1 = rtu_Encode_Pos;
2275
2276 /* Gain: '<S8>/GHDG5' */
2277 localB->GHDG5 = localP->GHDG5_Gain * localB->DataTypeConversion1;
2278
2279 /* Gain: '<S8>/fk_dg1' */
2280 localB->fk_dg1 = localP->fk_dg1_Gain * localB->GHDG5;
2281
2282 /* Gain: '<S8>/fk_dg' */
2283 localB->fk_dg = localP->fk_dg_Gain * localB->fk_dg1;
2284
2285 /* Sum: '<S8>/Sum' */
2286 localB->Sum = localB->GHDG4 - localB->fk_dg;
2287
2288 /* Saturate: '<S8>/Saturation1' */
2289 u0 = localB->Sum;
2290 u1 = localP->Saturation1_LowerSat;
2291 u2 = localP->Saturation1_UpperSat;
2292 if (u0 > u2) {
2293 localB->Saturation1 = u2;
2294 } else if (u0 < u1) {
2295 localB->Saturation1 = u1;
2296 } else {
2297 localB->Saturation1 = u0;
2298 }
2299
2300 /* End of Saturate: '<S8>/Saturation1' */
2301
2302 /* Gain: '<S42>/ZP_Kp' */
2303 localB->ZP_Kp = localP->ZP_Kp_Gain * localB->Saturation1;
2304
2305 /* Gain: '<S42>/ZV_KD1' */
2306 localB->ZV_KD1 = localP->ZV_KD1_Gain * localB->ZP_Kp;
2307
2308 /* Sum: '<S42>/Sum' */
2309 localB->Sum_i = localB->ZV_KD1 - localB->DiscreteTimeIntegrator;
2310
2311 /* Gain: '<S42>/ZP_KN' */
2312 localB->ZP_KN = localP->ZP_KN_Gain * localB->Sum_i;
2313
2314 /* Gain: '<S42>/KD_KG' */
2315 localB->KD_KG = localP->KD_KG_Gain * localB->ZP_KN;
2316
2317 /* DataStoreRead: '<S42>/P_KP' */
2318 localB->P_KP = *rtd_P_KP;
2319
2320 /* Product: '<S42>/Product' */
2321 localB->Product_i = localB->ZP_Kp * localB->P_KP;
2322
2323 /* Sum: '<S42>/Sum1' */
2324 localB->Sum1_n = localB->Product_i + localB->KD_KG;
2325
2326 /* DataStoreRead: '<S43>/KG_clc' */
2327 localB->KG_clc = *rtd_KG_clc;
2328
2329 /* DiscreteIntegrator: '<S43>/Discrete-Time Integrator' */
2330 if (((localB->KG_clc > 0) && (localDW->DiscreteTimeIntegrator_PrevRese <= 0)) ||
2331 ((localB->KG_clc <= 0) && (localDW->DiscreteTimeIntegrator_PrevRese == 1)))
2332 {
2333 localDW->DiscreteTimeIntegrator_DSTATE_a =
2334 localP->DiscreteTimeIntegrator_IC_g;
2335 }
2336
2337 if (localDW->DiscreteTimeIntegrator_DSTATE_a >=
2338 localP->DiscreteTimeIntegrator_UpperS_j) {
2339 localDW->DiscreteTimeIntegrator_DSTATE_a =
2340 localP->DiscreteTimeIntegrator_UpperS_j;
2341 } else {
2342 if (localDW->DiscreteTimeIntegrator_DSTATE_a <=
2343 localP->DiscreteTimeIntegrator_LowerS_o) {
2344 localDW->DiscreteTimeIntegrator_DSTATE_a =
2345 localP->DiscreteTimeIntegrator_LowerS_o;
2346 }
2347 }
2348
2349 localB->DiscreteTimeIntegrator_d = localDW->DiscreteTimeIntegrator_DSTATE_a;
2350
2351 /* End of DiscreteIntegrator: '<S43>/Discrete-Time Integrator' */
2352
2353 /* DiscreteIntegrator: '<S43>/Discrete-Time Integrator1' */
2354 localB->DiscreteTimeIntegrator1 = localDW->DiscreteTimeIntegrator1_DSTATE;
2355
2356 /* DataStoreRead: '<S8>/Saturation_limit_speed' */
2357 localB->Saturation_limit_speed = *rtd_Saturation_limit_speed;
2358
2359 /* RelationalOperator: '<S48>/LowerRelop1' */
2360 localB->LowerRelop1 = (localB->Sum1_n > localB->Saturation_limit_speed);
2361
2362 /* Gain: '<S8>/KP_e2' */
2363 localB->KP_e2 = localP->KP_e2_Gain * localB->Saturation_limit_speed;
2364
2365 /* RelationalOperator: '<S48>/UpperRelop' */
2366 localB->UpperRelop = (localB->Sum1_n < localB->KP_e2);
2367
2368 /* Switch: '<S48>/Switch' */
2369 if (localB->UpperRelop) {
2370 localB->Switch = localB->KP_e2;
2371 } else {
2372 localB->Switch = localB->Sum1_n;
2373 }
2374
2375 /* End of Switch: '<S48>/Switch' */
2376
2377 /* Switch: '<S48>/Switch2' */
2378 if (localB->LowerRelop1) {
2379 localB->Switch2 = localB->Saturation_limit_speed;
2380 } else {
2381 localB->Switch2 = localB->Switch;
2382 }
2383
2384 /* End of Switch: '<S48>/Switch2' */
2385
2386 /* UnitDelay: '<S8>/Unit Delay' */
2387 localB->UnitDelay = localDW->UnitDelay_DSTATE;
2388
2389 /* Sum: '<S8>/Sum5' */
2390 localB->Sum5 = localB->fk_dg1 - localB->UnitDelay;
2391
2392 /* Gain: '<S8>/GXZ5' */
2393 localB->GXZ5 = localP->GXZ5_Gain * localB->Sum5;
2394
2395 /* Gain: '<S8>/fk_dg2' */
2396 localB->fk_dg2 = localP->fk_dg2_Gain * localB->GXZ5;
2397
2398 /* Sum: '<S8>/Sum2' */
2399 localB->Sum2 = localB->Switch2 - localB->fk_dg2;
2400
2401 /* Gain: '<S43>/ZV_Kp' */
2402 localB->ZV_Kp = localP->ZV_Kp_Gain * localB->Sum2;
2403
2404 /* Gain: '<S43>/ZV_Kp1' */
2405 localB->ZV_Kp1 = localP->ZV_Kp1_Gain * localB->ZV_Kp;
2406
2407 /* Sum: '<S43>/Sum' */
2408 localB->Sum_o = localB->ZV_Kp1 - localB->DiscreteTimeIntegrator1;
2409
2410 /* Gain: '<S43>/ZV_KN' */
2411 localB->ZV_KN = localP->ZV_KN_Gain * localB->Sum_o;
2412
2413 /* Gain: '<S43>/KD_KG' */
2414 localB->KD_KG_h = localP->KD_KG_Gain_e * localB->ZV_KN;
2415
2416 /* DataStoreRead: '<S43>/V_KP' */
2417 localB->V_KP = *rtd_V_KP;
2418
2419 /* Product: '<S43>/Product' */
2420 localB->Product_i4 = localB->ZV_Kp * localB->V_KP;
2421
2422 /* DataStoreRead: '<S43>/V_KI' */
2423 localB->V_KI = *rtd_V_KI;
2424
2425 /* Product: '<S43>/Product1' */
2426 localB->Product1 = localB->DiscreteTimeIntegrator_d * localB->V_KI;
2427
2428 /* Signum: '<S43>/Sign' */
2429 u0 = localB->Switch2;
2430 if (u0 < 0.0) {
2431 localB->Sign = -1.0;
2432 } else if (u0 > 0.0) {
2433 localB->Sign = 1.0;
2434 } else {
2435 localB->Sign = u0;
2436 }
2437
2438 /* End of Signum: '<S43>/Sign' */
2439
2440 /* Gain: '<S43>/ZV1_Kp1' */
2441 localB->ZV1_Kp1 = localP->ZV1_Kp1_Gain * localB->Switch2;
2442
2443 /* Gain: '<S43>/ZV1_Kp2' */
2444 localB->ZV1_Kp2 = localP->ZV1_Kp2_Gain * localB->Sign;
2445
2446 /* Sum: '<S43>/Sum2' */
2447 localB->Sum2_f = (localB->Product1 + localB->Product_i4) + localB->KD_KG_h;
2448
2449 /* Sum: '<S43>/Sum1' */
2450 localB->Sum1_ne = (localB->ZV1_Kp1 + localB->ZV1_Kp2) + localB->Sum2_f;
2451
2452 /* Saturate: '<S8>/XF_PID' */
2453 u0 = localB->Sum1_ne;
2454 u1 = localP->XF_PID_LowerSat;
2455 u2 = localP->XF_PID_UpperSat;
2456 if (u0 > u2) {
2457 localB->XF_PID = u2;
2458 } else if (u0 < u1) {
2459 localB->XF_PID = u1;
2460 } else {
2461 localB->XF_PID = u0;
2462 }
2463
2464 /* End of Saturate: '<S8>/XF_PID' */
2465
2466 /* Sum: '<S8>/Sum3' incorporates:
2467 * Constant: '<S8>/Constant'
2468 */
2469 localB->Sum3 = localB->XF_PID + localP->Constant_Value;
2470
2471 /* Saturate: '<S8>/XF_PWM' */
2472 u0 = localB->Sum3;
2473 u1 = localP->XF_PWM_LowerSat;
2474 u2 = localP->XF_PWM_UpperSat;
2475 if (u0 > u2) {
2476 localB->XF_PWM = u2;
2477 } else if (u0 < u1) {
2478 localB->XF_PWM = u1;
2479 } else {
2480 localB->XF_PWM = u0;
2481 }
2482
2483 /* End of Saturate: '<S8>/XF_PWM' */
2484
2485 /* DataTypeConversion: '<S8>/Data Type Conversion3' */
2486 u0 = fmod(floor(localB->XF_PWM), 65536.0);
2487 localB->DataTypeConversion3 = (uint16_T)(u0 < 0.0 ? (int32_T)(uint16_T)
2488 -(int16_T)(uint16_T)-u0 : (int32_T)(uint16_T)u0);
2489
2490 /* Gain: '<S8>/GHDG7' */
2491 localB->GHDG7 = localP->GHDG7_Gain * localB->GHDG5;
2492
2493 /* Gain: '<S8>/GHDG8' */
2494 localB->GHDG8 = localP->GHDG8_Gain * localB->GHDG7;
2495
2496 /* Fcn: '<S49>/Fcn7' */
2497 localB->Fcn7 = localB->GHDG8 + 0.2655;
2498
2499 /* Fcn: '<S49>/Cl1' */
2500 localB->Cl1 = (0.090490250000000008 - localB->Fcn7 * localB->Fcn7) /
2501 0.020000000000000004;
2502
2503 /* Fcn: '<S49>/Fcn' */
2504 u0 = (7.0490249999999985 - localB->Cl1 * localB->Cl1) + 1.0;
2505 if (u0 < 0.0) {
2506 u0 = -sqrt(-u0);
2507 } else {
2508 u0 = sqrt(u0);
2509 }
2510
2511 localB->Fcn_l = (2.655 - u0) / (localB->Cl1 + 1.0);
2512
2513 /* End of Fcn: '<S49>/Fcn' */
2514
2515 /* Fcn: '<S49>/Fcn4' */
2516 localB->Fcn4_g = 2.0 * atan(localB->Fcn_l);
2517
2518 /* Gain: '<S8>/GHDG9' */
2519 localB->GHDG9 = localP->GHDG9_Gain * localB->Fcn4_g;
2520
2521 /* Gain: '<S8>/fk_dg3' */
2522 localB->fk_dg3 = localP->fk_dg3_Gain * localB->GHDG9;
2523
2524 /* Gain: '<S8>/KP_JD' */
2525 localB->KP_JD = localP->KP_JD_Gain * localB->fk_dg3;
2526
2527 /* Sum: '<S8>/Sum1' */
2528 localB->Sum1_h = localB->Sum4 - localB->fk_dg3;
2529
2530 /* Gain: '<S8>/KP_e' */
2531 localB->KP_e = localP->KP_e_Gain * localB->Sum1_h;
2532
2533 /* If: '<S8>/If' */
2534 if (rtu_Slect_port == 5) {
2535 /* Outputs for IfAction SubSystem: '<S8>/If Action Subsystem' incorporates:
2536 * ActionPort: '<S44>/Action Port'
2537 */
2538 /* DataStoreWrite: '<S44>/Data Store Write' */
2539 *rtd_JD_HYDG_HC = rtu_JD_In;
2540
2541 /* DataStoreWrite: '<S44>/Data Store Write1' */
2542 *rtd_JD_HYDG = rtu_JD_In;
2543
2544 /* End of Outputs for SubSystem: '<S8>/If Action Subsystem' */
2545 } else if (rtu_Slect_port == 4) {
2546 /* Outputs for IfAction SubSystem: '<S8>/If Action Subsystem1' incorporates:
2547 * ActionPort: '<S45>/Action Port'
2548 */
2549 /* DataStoreRead: '<S45>/Data Store Read' */
2550 localB->DataStoreRead_a = *rtd_JD_HYDG_HC;
2551
2552 /* DataStoreWrite: '<S45>/Data Store Write1' */
2553 *rtd_JD_HYDG = localB->DataStoreRead_a;
2554
2555 /* End of Outputs for SubSystem: '<S8>/If Action Subsystem1' */
2556 } else {
2557 /* Outputs for IfAction SubSystem: '<S8>/If Action Subsystem2' incorporates:
2558 * ActionPort: '<S46>/Action Port'
2559 */
2560 /* DataStoreWrite: '<S46>/Data Store Write' */
2561 *rtd_JD_HYDG_HC = rtu_JD_In;
2562
2563 /* DataStoreWrite: '<S46>/Data Store Write1' */
2564 *rtd_JD_HYDG = rtu_JD_In;
2565
2566 /* End of Outputs for SubSystem: '<S8>/If Action Subsystem2' */
2567 }
2568
2569 /* End of If: '<S8>/If' */
2570
2571 /* Update for DiscreteIntegrator: '<S42>/Discrete-Time Integrator' */
2572 localDW->DiscreteTimeIntegrator_DSTATE +=
2573 localP->DiscreteTimeIntegrator_gainval * (real_T)
2574 localDW->Motor_HYDG1_ELAPS_T[0] * localB->ZP_KN;
2575 if (localDW->DiscreteTimeIntegrator_DSTATE >=
2576 localP->DiscreteTimeIntegrator_UpperSat) {
2577 localDW->DiscreteTimeIntegrator_DSTATE =
2578 localP->DiscreteTimeIntegrator_UpperSat;
2579 } else {
2580 if (localDW->DiscreteTimeIntegrator_DSTATE <=
2581 localP->DiscreteTimeIntegrator_LowerSat) {
2582 localDW->DiscreteTimeIntegrator_DSTATE =
2583 localP->DiscreteTimeIntegrator_LowerSat;
2584 }
2585 }
2586
2587 /* End of Update for DiscreteIntegrator: '<S42>/Discrete-Time Integrator' */
2588
2589 /* Update for DiscreteIntegrator: '<S43>/Discrete-Time Integrator' */
2590 localDW->DiscreteTimeIntegrator_DSTATE_a +=
2591 localP->DiscreteTimeIntegrator_gainva_p * (real_T)
2592 localDW->Motor_HYDG1_ELAPS_T[0] * localB->ZV_Kp;
2593 if (localDW->DiscreteTimeIntegrator_DSTATE_a >=
2594 localP->DiscreteTimeIntegrator_UpperS_j) {
2595 localDW->DiscreteTimeIntegrator_DSTATE_a =
2596 localP->DiscreteTimeIntegrator_UpperS_j;
2597 } else {
2598 if (localDW->DiscreteTimeIntegrator_DSTATE_a <=
2599 localP->DiscreteTimeIntegrator_LowerS_o) {
2600 localDW->DiscreteTimeIntegrator_DSTATE_a =
2601 localP->DiscreteTimeIntegrator_LowerS_o;
2602 }
2603 }
2604
2605 localDW->DiscreteTimeIntegrator_PrevRese = (int8_T)(localB->KG_clc > 0);
2606
2607 /* End of Update for DiscreteIntegrator: '<S43>/Discrete-Time Integrator' */
2608
2609 /* Update for DiscreteIntegrator: '<S43>/Discrete-Time Integrator1' */
2610 localDW->DiscreteTimeIntegrator1_DSTATE +=
2611 localP->DiscreteTimeIntegrator1_gainval * (real_T)
2612 localDW->Motor_HYDG1_ELAPS_T[0] * localB->ZV_KN;
2613 if (localDW->DiscreteTimeIntegrator1_DSTATE >=
2614 localP->DiscreteTimeIntegrator1_UpperSa) {
2615 localDW->DiscreteTimeIntegrator1_DSTATE =
2616 localP->DiscreteTimeIntegrator1_UpperSa;
2617 } else {
2618 if (localDW->DiscreteTimeIntegrator1_DSTATE <=
2619 localP->DiscreteTimeIntegrator1_LowerSa) {
2620 localDW->DiscreteTimeIntegrator1_DSTATE =
2621 localP->DiscreteTimeIntegrator1_LowerSa;
2622 }
2623 }
2624
2625 /* End of Update for DiscreteIntegrator: '<S43>/Discrete-Time Integrator1' */
2626
2627 /* Update for UnitDelay: '<S8>/Unit Delay' */
2628 localDW->UnitDelay_DSTATE = localB->fk_dg1;
2629}
2630
2631/* Output and update for function-call system: '<S1>/Showing' */
2632void motor_Showing(RT_MODEL_motor * const motor_M, real_T rtu_Showing_Angle,
2633 real_T rtu_Showing_T, rtB_Showing_motor *localB,
2634 rtP_Showing_motor *localP, real_T *rtd_Angle_S)
2635{
2636 /* MultiPortSwitch: '<S12>/Multiport Switch' */
2637 switch ((int32_T)rtu_Showing_T) {
2638 case 1:
2639 /* Sin: '<S12>/Sine Wave1' */
2640 localB->SineWave1 = sin(localP->SineWave1_Freq *
2641 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
2642 0.002) + localP->SineWave1_Phase) * localP->SineWave1_Amp +
2643 localP->SineWave1_Bias;
2644 localB->MultiportSwitch = localB->SineWave1;
2645 break;
2646
2647 case 2:
2648 /* Sin: '<S12>/Sine Wave2' */
2649 localB->SineWave2 = sin(localP->SineWave2_Freq *
2650 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
2651 0.002) + localP->SineWave2_Phase) * localP->SineWave2_Amp +
2652 localP->SineWave2_Bias;
2653 localB->MultiportSwitch = localB->SineWave2;
2654 break;
2655
2656 case 3:
2657 /* Sin: '<S12>/Sine Wave3' */
2658 localB->SineWave3 = sin(localP->SineWave3_Freq *
2659 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
2660 0.002) + localP->SineWave3_Phase) * localP->SineWave3_Amp +
2661 localP->SineWave3_Bias;
2662 localB->MultiportSwitch = localB->SineWave3;
2663 break;
2664
2665 case 4:
2666 /* Sin: '<S12>/Sine Wave4' */
2667 localB->SineWave4 = sin(localP->SineWave4_Freq *
2668 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
2669 0.002) + localP->SineWave4_Phase) * localP->SineWave4_Amp +
2670 localP->SineWave4_Bias;
2671 localB->MultiportSwitch = localB->SineWave4;
2672 break;
2673
2674 case 5:
2675 /* Sin: '<S12>/Sine Wave5' */
2676 localB->SineWave5 = sin(localP->SineWave5_Freq *
2677 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
2678 0.002) + localP->SineWave5_Phase) * localP->SineWave5_Amp +
2679 localP->SineWave5_Bias;
2680 localB->MultiportSwitch = localB->SineWave5;
2681 break;
2682
2683 case 6:
2684 /* Sin: '<S12>/Sine Wave6' */
2685 localB->SineWave6 = sin(localP->SineWave6_Freq *
2686 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
2687 0.002) + localP->SineWave6_Phase) * localP->SineWave6_Amp +
2688 localP->SineWave6_Bias;
2689 localB->MultiportSwitch = localB->SineWave6;
2690 break;
2691
2692 case 7:
2693 /* Sin: '<S12>/Sine Wave7' */
2694 localB->SineWave7 = sin(localP->SineWave7_Freq *
2695 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
2696 0.002) + localP->SineWave7_Phase) * localP->SineWave7_Amp +
2697 localP->SineWave7_Bias;
2698 localB->MultiportSwitch = localB->SineWave7;
2699 break;
2700
2701 default:
2702 /* Sin: '<S12>/Sine Wave8' */
2703 localB->SineWave8 = sin(localP->SineWave8_Freq *
2704 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
2705 0.002) + localP->SineWave8_Phase) * localP->SineWave8_Amp +
2706 localP->SineWave8_Bias;
2707 localB->MultiportSwitch = localB->SineWave8;
2708 break;
2709 }
2710
2711 /* End of MultiPortSwitch: '<S12>/Multiport Switch' */
2712
2713 /* DataStoreWrite: '<S12>/Data Store Write' */
2714 *rtd_Angle_S = localB->MultiportSwitch;
2715
2716 /* DataStoreRead: '<S12>/Data Store Read' */
2717 localB->DataStoreRead = *rtd_Angle_S;
2718
2719 /* Product: '<S12>/Product' */
2720 localB->Product = localB->DataStoreRead * rtu_Showing_Angle;
2721
2722 /* DataTypeConversion: '<S12>/Data Type Conversion1' */
2723 localB->DataTypeConversion1 = localB->Product;
2724}
2725
2726/* Output and update for function-call system: '<S1>/Angle_Calculation_HY' */
2727void motor_Angle_Calculation_HY(int32_T rtu_Encode_Pos,
2728 rtB_Angle_Calculation_HY_motor *localB, rtP_Angle_Calculation_HY_motor *localP)
2729{
2730 real_T tmp;
2731
2732 /* DataTypeConversion: '<S3>/Data Type Conversion1' */
2733 localB->DataTypeConversion1 = rtu_Encode_Pos;
2734
2735 /* Gain: '<S3>/GHDG5' */
2736 localB->GHDG5 = localP->GHDG5_Gain * localB->DataTypeConversion1;
2737
2738 /* Gain: '<S21>/GHDG7' */
2739 localB->GHDG7 = localP->GHDG7_Gain * localB->GHDG5;
2740
2741 /* Gain: '<S21>/GHDG8' */
2742 localB->GHDG8 = localP->GHDG8_Gain * localB->GHDG7;
2743
2744 /* Fcn: '<S22>/Fcn7' */
2745 localB->Fcn7 = localB->GHDG8 + 0.2655;
2746
2747 /* Fcn: '<S22>/Cl1' */
2748 localB->Cl1 = (0.090490250000000008 - localB->Fcn7 * localB->Fcn7) /
2749 0.020000000000000004;
2750
2751 /* Fcn: '<S22>/Fcn' */
2752 tmp = (7.0490249999999985 - localB->Cl1 * localB->Cl1) + 1.0;
2753 if (tmp < 0.0) {
2754 tmp = -sqrt(-tmp);
2755 } else {
2756 tmp = sqrt(tmp);
2757 }
2758
2759 localB->Fcn = (2.655 - tmp) / (localB->Cl1 + 1.0);
2760
2761 /* End of Fcn: '<S22>/Fcn' */
2762
2763 /* Fcn: '<S22>/Fcn4' */
2764 localB->Fcn4 = 2.0 * atan(localB->Fcn);
2765
2766 /* Gain: '<S21>/GHDG9' */
2767 localB->GHDG9 = localP->GHDG9_Gain * localB->Fcn4;
2768}
2769
2770/* Initial conditions for function-call system: '<S1>/Waveform_Build' */
2771void motor_Waveform_Build_Init(RT_MODEL_motor * const motor_M,
2772 rtDW_Waveform_Build_motor *localDW, rtP_Waveform_Build_motor *localP)
2773{
2774 uint32_T prevT_idx_0;
2775 uint32_T prevT_idx_1;
2776 prevT_idx_0 = motor_M->Timing.clockTick0;
2777 prevT_idx_1 = motor_M->Timing.clockTickH0;
2778 localDW->Waveform_Build_PREV_T[0] = prevT_idx_0;
2779 localDW->Waveform_Build_PREV_T[1] = prevT_idx_1;
2780
2781 /* InitializeConditions for Delay: '<S14>/Delay5' */
2782 localDW->Delay5_DSTATE = localP->Delay5_InitialCondition;
2783
2784 /* InitializeConditions for Delay: '<S14>/Delay7' */
2785 localDW->Delay7_DSTATE = localP->Delay7_InitialCondition;
2786
2787 /* InitializeConditions for Delay: '<S14>/Delay6' */
2788 localDW->Delay6_DSTATE = localP->Delay6_InitialCondition;
2789
2790 /* InitializeConditions for UnitDelay: '<S71>/Delay Input2' */
2791 localDW->DelayInput2_DSTATE = localP->DelayInput2_InitialCondition;
2792}
2793
2794/* Start for function-call system: '<S1>/Waveform_Build' */
2795void motor_Waveform_Build_Start(rtDW_Waveform_Build_motor *localDW)
2796{
2797 /* Start for If: '<S14>/If' */
2798 localDW->If_ActiveSubsystem = -1;
2799}
2800
2801/* Output and update for function-call system: '<S1>/Waveform_Build' */
2802void motor_Waveform_Build(RT_MODEL_motor * const motor_M, int32_T rtu_JD_In,
2803 rtB_Waveform_Build_motor *localB, rtDW_Waveform_Build_motor *localDW,
2804 rtP_Waveform_Build_motor *localP, real_T *rtd_T, real_T *rtd_T_Count, real_T
2805 *rtd_Temp1, real_T *rtd_Temp2)
2806{
2807 uint32_T elapseT_H;
2808 int8_T rtAction;
2809 real_T elapseTime;
2810 uint32_T prevT_idx_0;
2811 uint32_T elapseTime_idx_0;
2812 uint32_T prevT_idx_1;
2813 real_T u1;
2814 real_T u2;
2815 prevT_idx_0 = localDW->Waveform_Build_PREV_T[0];
2816 prevT_idx_1 = localDW->Waveform_Build_PREV_T[1];
2817 elapseTime_idx_0 = motor_M->Timing.clockTick0 - prevT_idx_0;
2818 elapseT_H = motor_M->Timing.clockTickH0 - prevT_idx_1;
2819 if (prevT_idx_0 > motor_M->Timing.clockTick0) {
2820 elapseT_H--;
2821 }
2822
2823 prevT_idx_0 = motor_M->Timing.clockTick0;
2824 prevT_idx_1 = motor_M->Timing.clockTickH0;
2825 localDW->Waveform_Build_ELAPS_T[0] = elapseTime_idx_0;
2826 localDW->Waveform_Build_PREV_T[0] = prevT_idx_0;
2827 localDW->Waveform_Build_ELAPS_T[1] = elapseT_H;
2828 localDW->Waveform_Build_PREV_T[1] = prevT_idx_1;
2829
2830 /* Delay: '<S14>/Delay5' */
2831 localB->Delay5 = localDW->Delay5_DSTATE;
2832
2833 /* DataTypeConversion: '<S14>/Data Type Conversion4' */
2834 localB->DataTypeConversion4 = rtu_JD_In;
2835
2836 /* Gain: '<S14>/Gain1' */
2837 localB->Gain1 = localP->Gain1_Gain * localB->DataTypeConversion4;
2838
2839 /* Saturate: '<S14>/XF_XHZY' */
2840 elapseTime = localB->Gain1;
2841 u1 = localP->XF_XHZY_LowerSat;
2842 u2 = localP->XF_XHZY_UpperSat;
2843 if (elapseTime > u2) {
2844 localB->XF_XHZY = u2;
2845 } else if (elapseTime < u1) {
2846 localB->XF_XHZY = u1;
2847 } else {
2848 localB->XF_XHZY = elapseTime;
2849 }
2850
2851 /* End of Saturate: '<S14>/XF_XHZY' */
2852
2853 /* Sum: '<S14>/Add16' */
2854 localB->Add16 = localB->XF_XHZY - localB->Delay5;
2855
2856 /* If: '<S14>/If' incorporates:
2857 * Inport: '<S67>/In1'
2858 */
2859 rtAction = (int8_T)!(localB->Add16 != 0.0);
2860 localDW->If_ActiveSubsystem = rtAction;
2861 switch (rtAction) {
2862 case 0:
2863 /* Outputs for IfAction SubSystem: '<S14>/If Action Subsystem1' incorporates:
2864 * ActionPort: '<S67>/Action Port'
2865 */
2866 /* DataStoreWrite: '<S67>/Data Store Write2' */
2867 *rtd_Temp1 = localB->Delay5;
2868
2869 /* DataStoreRead: '<S67>/Data Store Read5' */
2870 localB->DataStoreRead5_p = *rtd_T_Count;
2871
2872 /* MATLAB Function: '<S67>/MATLAB Function' */
2873 /* MATLAB Function 'Chart/Waveform_Build/If Action Subsystem1/MATLAB Function': '<S73>:1' */
2874 /* '<S73>:1:4' */
2875 localB->y1 = localB->DataStoreRead5_p + 1.0;
2876
2877 /* '<S73>:1:5' */
2878 localB->y2 = 0.0;
2879
2880 /* DataStoreWrite: '<S67>/Data Store Write1' */
2881 *rtd_T_Count = localB->y2;
2882
2883 /* DataStoreWrite: '<S67>/Data Store Write3' */
2884 *rtd_T = localB->y1;
2885 localB->In1_m = localB->Add16;
2886
2887 /* End of Outputs for SubSystem: '<S14>/If Action Subsystem1' */
2888 break;
2889
2890 case 1:
2891 /* Outputs for IfAction SubSystem: '<S14>/If Action Subsystem3' incorporates:
2892 * ActionPort: '<S69>/Action Port'
2893 */
2894 /* DataStoreRead: '<S69>/Data Store Read5' */
2895 localB->DataStoreRead5_m = *rtd_T_Count;
2896
2897 /* Sum: '<S69>/Add20' incorporates:
2898 * Constant: '<S69>/Constant3'
2899 */
2900 localB->Add20_j = localB->DataStoreRead5_m + localP->Constant3_Value;
2901
2902 /* Saturate: '<S69>/Saturation' */
2903 elapseTime = localB->Add20_j;
2904 u1 = localP->Saturation_LowerSat;
2905 u2 = localP->Saturation_UpperSat;
2906 if (elapseTime > u2) {
2907 localB->Saturation = u2;
2908 } else if (elapseTime < u1) {
2909 localB->Saturation = u1;
2910 } else {
2911 localB->Saturation = elapseTime;
2912 }
2913
2914 /* End of Saturate: '<S69>/Saturation' */
2915
2916 /* DataStoreWrite: '<S69>/Data Store Write1' */
2917 *rtd_T_Count = localB->Saturation;
2918 break;
2919 }
2920
2921 /* End of If: '<S14>/If' */
2922
2923 /* Delay: '<S14>/Delay7' */
2924 localB->Delay7 = localDW->Delay7_DSTATE;
2925
2926 /* DataStoreRead: '<S14>/Data Store Read1' */
2927 localB->DataStoreRead1 = *rtd_T_Count;
2928
2929 /* Delay: '<S14>/Delay6' */
2930 localB->Delay6 = localDW->Delay6_DSTATE;
2931
2932 /* DataStoreRead: '<S14>/Data Store Read4' */
2933 localB->DataStoreRead4 = *rtd_Temp1;
2934
2935 /* Sum: '<S14>/Add21' */
2936 localB->Add21 = localB->DataStoreRead4 - localB->Delay6;
2937
2938 /* If: '<S14>/If1' incorporates:
2939 * Inport: '<S68>/In1'
2940 */
2941 if (localB->Add21 != 0.0) {
2942 /* Outputs for IfAction SubSystem: '<S14>/If Action Subsystem2' incorporates:
2943 * ActionPort: '<S68>/Action Port'
2944 */
2945 /* DataStoreWrite: '<S68>/Data Store Write2' */
2946 *rtd_Temp2 = localB->Delay6;
2947 localB->In1_n = localB->Add21;
2948
2949 /* End of Outputs for SubSystem: '<S14>/If Action Subsystem2' */
2950 }
2951
2952 /* End of If: '<S14>/If1' */
2953
2954 /* Switch: '<S14>/Switch' */
2955 if (localB->DataStoreRead1 > localP->Switch_Threshold) {
2956 localB->Switch = localB->XF_XHZY;
2957 } else {
2958 /* Gain: '<S14>/Gain7' */
2959 localB->Gain7 = localP->Gain7_Gain * localB->In1_m;
2960
2961 /* Sum: '<S14>/Add12' */
2962 localB->Add12 = localB->In1_m - localB->In1_n;
2963
2964 /* Sum: '<S14>/Add19' */
2965 localB->Add19 = (localB->Add12 + localB->Gain7) + localB->XF_XHZY;
2966 localB->Switch = localB->Add19;
2967 }
2968
2969 /* End of Switch: '<S14>/Switch' */
2970
2971 /* Sum: '<S14>/Add20' */
2972 localB->Add20 = localB->Switch - localB->Delay7;
2973
2974 /* If: '<S14>/If2' incorporates:
2975 * Inport: '<S70>/In1'
2976 */
2977 if (localB->Add20 != 0.0) {
2978 /* Outputs for IfAction SubSystem: '<S14>/If Action Subsystem4' incorporates:
2979 * ActionPort: '<S70>/Action Port'
2980 */
2981 localB->In1 = localB->Add20;
2982
2983 /* End of Outputs for SubSystem: '<S14>/If Action Subsystem4' */
2984 }
2985
2986 /* End of If: '<S14>/If2' */
2987
2988 /* Abs: '<S14>/Abs2' */
2989 localB->Abs2 = fabs(localB->In1);
2990
2991 /* DataStoreRead: '<S14>/Data Store Read5' */
2992 localB->DataStoreRead5 = *rtd_T;
2993
2994 /* Product: '<S14>/Divide1' incorporates:
2995 * Constant: '<S14>/Constant2'
2996 */
2997 localB->Divide1 = localB->DataStoreRead5 / localP->Constant2_Value;
2998
2999 /* Saturate: '<S14>/Saturation2' */
3000 elapseTime = localB->Divide1;
3001 u1 = localP->Saturation2_LowerSat;
3002 u2 = localP->Saturation2_UpperSat;
3003 if (elapseTime > u2) {
3004 localB->Saturation2 = u2;
3005 } else if (elapseTime < u1) {
3006 localB->Saturation2 = u1;
3007 } else {
3008 localB->Saturation2 = elapseTime;
3009 }
3010
3011 /* End of Saturate: '<S14>/Saturation2' */
3012
3013 /* Sum: '<S14>/Add13' */
3014 localB->Add13 = localB->XF_XHZY - localB->Saturation2;
3015
3016 /* Sum: '<S14>/Add15' */
3017 localB->Add15 = localB->Saturation2 + localB->XF_XHZY;
3018
3019 /* Product: '<S14>/Divide' incorporates:
3020 * Constant: '<S14>/Constant3'
3021 */
3022 localB->Divide = localP->Constant3_Value_a / localB->DataStoreRead5;
3023
3024 /* Product: '<S14>/Product1' */
3025 localB->Product1 = localB->Divide * localB->Abs2;
3026
3027 /* Saturate: '<S14>/Saturation1' */
3028 elapseTime = localB->Product1;
3029 u1 = localP->Saturation1_LowerSat;
3030 u2 = localP->Saturation1_UpperSat;
3031 if (elapseTime > u2) {
3032 localB->Saturation1 = u2;
3033 } else if (elapseTime < u1) {
3034 localB->Saturation1 = u1;
3035 } else {
3036 localB->Saturation1 = elapseTime;
3037 }
3038
3039 /* End of Saturate: '<S14>/Saturation1' */
3040
3041 /* SampleTimeMath: '<S71>/sample time'
3042 *
3043 * About '<S71>/sample time':
3044 * y = K where K = ( w * Ts )
3045 */
3046 elapseTime = ((real_T)localDW->Waveform_Build_ELAPS_T[0] * 0.002 + (real_T)
3047 localDW->Waveform_Build_ELAPS_T[1] * 8.589934592E+6) *
3048 localP->sampletime_WtEt;
3049 localB->sampletime = elapseTime;
3050
3051 /* Product: '<S71>/delta rise limit' */
3052 localB->deltariselimit = localB->Saturation1 * localB->sampletime;
3053
3054 /* UnitDelay: '<S71>/Delay Input2' */
3055 localB->Yk1 = localDW->DelayInput2_DSTATE;
3056
3057 /* Sum: '<S71>/Difference Inputs1' */
3058 localB->UkYk1 = localB->Switch - localB->Yk1;
3059
3060 /* RelationalOperator: '<S74>/LowerRelop1' */
3061 localB->LowerRelop1 = (localB->UkYk1 > localB->deltariselimit);
3062
3063 /* Gain: '<S14>/Gain3' */
3064 localB->Gain3 = localP->Gain3_Gain * localB->Saturation1;
3065
3066 /* Product: '<S71>/delta fall limit' */
3067 localB->deltafalllimit = localB->Gain3 * localB->sampletime;
3068
3069 /* RelationalOperator: '<S74>/UpperRelop' */
3070 localB->UpperRelop = (localB->UkYk1 < localB->deltafalllimit);
3071
3072 /* Switch: '<S74>/Switch' */
3073 if (localB->UpperRelop) {
3074 localB->Switch_c = localB->deltafalllimit;
3075 } else {
3076 localB->Switch_c = localB->UkYk1;
3077 }
3078
3079 /* End of Switch: '<S74>/Switch' */
3080
3081 /* Switch: '<S74>/Switch2' */
3082 if (localB->LowerRelop1) {
3083 localB->Switch2 = localB->deltariselimit;
3084 } else {
3085 localB->Switch2 = localB->Switch_c;
3086 }
3087
3088 /* End of Switch: '<S74>/Switch2' */
3089
3090 /* Sum: '<S71>/Difference Inputs2' */
3091 localB->DifferenceInputs2 = localB->Switch2 + localB->Yk1;
3092
3093 /* RelationalOperator: '<S72>/LowerRelop1' */
3094 localB->LowerRelop1_g = (localB->DifferenceInputs2 > localB->Add15);
3095
3096 /* RelationalOperator: '<S72>/UpperRelop' */
3097 localB->UpperRelop_p = (localB->DifferenceInputs2 < localB->Add13);
3098
3099 /* Switch: '<S72>/Switch' */
3100 if (localB->UpperRelop_p) {
3101 localB->Switch_e = localB->Add13;
3102 } else {
3103 localB->Switch_e = localB->DifferenceInputs2;
3104 }
3105
3106 /* End of Switch: '<S72>/Switch' */
3107
3108 /* Switch: '<S72>/Switch2' */
3109 if (localB->LowerRelop1_g) {
3110 localB->Switch2_p = localB->Add15;
3111 } else {
3112 localB->Switch2_p = localB->Switch_e;
3113 }
3114
3115 /* End of Switch: '<S72>/Switch2' */
3116
3117 /* Gain: '<S14>/Gain2' */
3118 elapseTime = fmod(floor(localP->Gain2_Gain * localB->Switch2_p),
3119 4.294967296E+9);
3120 localB->Gain2 = elapseTime < 0.0 ? -(int32_T)(uint32_T)-elapseTime : (int32_T)
3121 (uint32_T)elapseTime;
3122
3123 /* Update for Delay: '<S14>/Delay5' */
3124 localDW->Delay5_DSTATE = localB->XF_XHZY;
3125
3126 /* Update for Delay: '<S14>/Delay7' */
3127 localDW->Delay7_DSTATE = localB->Switch;
3128
3129 /* Update for Delay: '<S14>/Delay6' */
3130 localDW->Delay6_DSTATE = localB->DataStoreRead4;
3131
3132 /* Update for UnitDelay: '<S71>/Delay Input2' */
3133 localDW->DelayInput2_DSTATE = localB->DifferenceInputs2;
3134}
3135
3136/* Output and update for function-call system: '<S1>/Showing_XHHY' */
3137void motor_Showing_XHHY(RT_MODEL_motor * const motor_M, real_T rtu_Showing_Angle,
3138 real_T rtu_Showing_T, rtB_Showing_XHHY_motor *localB, rtP_Showing_XHHY_motor
3139 *localP, real_T *rtd_Angle_S)
3140{
3141 /* MultiPortSwitch: '<S13>/Multiport Switch' */
3142 switch ((int32_T)rtu_Showing_T) {
3143 case 1:
3144 /* Sin: '<S13>/Sine Wave1' */
3145 localB->SineWave1 = sin(localP->SineWave1_Freq *
3146 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
3147 0.002) + localP->SineWave1_Phase) * localP->SineWave1_Amp +
3148 localP->SineWave1_Bias;
3149 localB->MultiportSwitch = localB->SineWave1;
3150 break;
3151
3152 case 2:
3153 /* Sin: '<S13>/Sine Wave2' */
3154 localB->SineWave2 = sin(localP->SineWave2_Freq *
3155 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
3156 0.002) + localP->SineWave2_Phase) * localP->SineWave2_Amp +
3157 localP->SineWave2_Bias;
3158 localB->MultiportSwitch = localB->SineWave2;
3159 break;
3160
3161 case 3:
3162 /* Sin: '<S13>/Sine Wave3' */
3163 localB->SineWave3 = sin(localP->SineWave3_Freq *
3164 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
3165 0.002) + localP->SineWave3_Phase) * localP->SineWave3_Amp +
3166 localP->SineWave3_Bias;
3167 localB->MultiportSwitch = localB->SineWave3;
3168 break;
3169
3170 case 4:
3171 /* Sin: '<S13>/Sine Wave4' */
3172 localB->SineWave4 = sin(localP->SineWave4_Freq *
3173 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
3174 0.002) + localP->SineWave4_Phase) * localP->SineWave4_Amp +
3175 localP->SineWave4_Bias;
3176 localB->MultiportSwitch = localB->SineWave4;
3177 break;
3178
3179 case 5:
3180 /* Sin: '<S13>/Sine Wave5' */
3181 localB->SineWave5 = sin(localP->SineWave5_Freq *
3182 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
3183 0.002) + localP->SineWave5_Phase) * localP->SineWave5_Amp +
3184 localP->SineWave5_Bias;
3185 localB->MultiportSwitch = localB->SineWave5;
3186 break;
3187
3188 case 6:
3189 /* Sin: '<S13>/Sine Wave6' */
3190 localB->SineWave6 = sin(localP->SineWave6_Freq *
3191 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
3192 0.002) + localP->SineWave6_Phase) * localP->SineWave6_Amp +
3193 localP->SineWave6_Bias;
3194 localB->MultiportSwitch = localB->SineWave6;
3195 break;
3196
3197 case 7:
3198 /* Sin: '<S13>/Sine Wave7' */
3199 localB->SineWave7 = sin(localP->SineWave7_Freq *
3200 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
3201 0.002) + localP->SineWave7_Phase) * localP->SineWave7_Amp +
3202 localP->SineWave7_Bias;
3203 localB->MultiportSwitch = localB->SineWave7;
3204 break;
3205
3206 default:
3207 /* Sin: '<S13>/Sine Wave8' */
3208 localB->SineWave8 = sin(localP->SineWave8_Freq *
3209 (((motor_M->Timing.clockTick0+motor_M->Timing.clockTickH0* 4294967296.0)) *
3210 0.002) + localP->SineWave8_Phase) * localP->SineWave8_Amp +
3211 localP->SineWave8_Bias;
3212 localB->MultiportSwitch = localB->SineWave8;
3213 break;
3214 }
3215
3216 /* End of MultiPortSwitch: '<S13>/Multiport Switch' */
3217
3218 /* DataStoreWrite: '<S13>/Data Store Write' */
3219 *rtd_Angle_S = localB->MultiportSwitch;
3220
3221 /* DataStoreRead: '<S13>/Data Store Read' */
3222 localB->DataStoreRead = *rtd_Angle_S;
3223
3224 /* Product: '<S13>/Product' */
3225 localB->Product = localB->DataStoreRead * rtu_Showing_Angle;
3226
3227 /* Gain: '<S13>/Gain' */
3228 localB->Gain = localP->Gain_Gain * localB->Product;
3229
3230 /* DataTypeConversion: '<S13>/Data Type Conversion1' */
3231 localB->DataTypeConversion1 = localB->Gain;
3232}
3233
3234/* Function for Chart: '<Root>/Chart' */
3235static uint8_T motor_Temp_Up_Check(uint16_T In1, uint16_T In2)
3236{
3237 uint8_T y;
3238
3239 /* Graphical Function 'Temp_Up_Check': '<S1>:52' */
3240 y = 0U;
3241
3242 /* Transition: '<S1>:440' */
3243 if ((!(In1 >= In2)) && (In1 < 2060)) {
3244 /* Transition: '<S1>:442' */
3245 y = 1U;
3246
3247 /* Transition: '<S1>:445' */
3248 } else {
3249 /* Transition: '<S1>:441' */
3250 /* Transition: '<S1>:443' */
3251 /* Transition: '<S1>:445' */
3252 /* Transition: '<S1>:1077' */
3253 }
3254
3255 return y;
3256}
3257
3258/* Function for Chart: '<Root>/Chart' */
3259static uint8_T motor_Temp_Down_Check(uint16_T In1, uint16_T In2)
3260{
3261 uint8_T y;
3262
3263 /* Graphical Function 'Temp_Down_Check': '<S1>:85' */
3264 y = 0U;
3265
3266 /* Transition: '<S1>:478' */
3267 if ((!(In1 <= In2)) && (In1 > 150)) {
3268 /* Transition: '<S1>:480' */
3269 y = 1U;
3270
3271 /* Transition: '<S1>:483' */
3272 } else {
3273 /* Transition: '<S1>:479' */
3274 /* Transition: '<S1>:481' */
3275 /* Transition: '<S1>:482' */
3276 /* Transition: '<S1>:483' */
3277 /* Transition: '<S1>:1078' */
3278 }
3279
3280 return y;
3281}
3282
3283/* Function for Chart: '<Root>/Chart' */
3284static void motor_defult(void)
3285{
3286 /* Inport: '<Root>/Test_Mode' incorporates:
3287 * Inport: '<Root>/Motor_Num'
3288 */
3289 /* During 'defult': '<S1>:239' */
3290 if (motor_U.Test_Mode == 1) {
3291 /* Transition: '<S1>:429' */
3292 motor_DWork.is_Test_Mode = motor_IN_Calibration;
3293
3294 /* Entry 'Calibration': '<S1>:237' */
3295 /* 标定模式 */
3296 motor_Y.DCZD = false;
3297
3298 /* 解除制动 */
3299 motor_Y.Motor_En = true;
3300
3301 /* 电机失能 */
3302 motor_DWork.chu_jd = 0.0;
3303 motor_DWork.KG = 1U;
3304 } else if (motor_U.Test_Mode == 2) {
3305 /* Transition: '<S1>:432' */
3306 motor_DWork.is_Test_Mode = motor_IN_Limit_Up_Test;
3307
3308 /* Inport: '<Root>/Motor_Num' */
3309 /* Entry 'Limit_Up_Test': '<S1>:238' */
3310 /* 测试电子上限位 */
3311 /* Entry Internal 'Limit_Up_Test': '<S1>:238' */
3312 /* Transition: '<S1>:664' */
3313 if (motor_U.Motor_Num == 1) {
3314 /* Transition: '<S1>:666' */
3315 motor_DWork.is_Limit_Up_Test = motor_IN_XHZY_pv;
3316
3317 /* Entry 'XHZY': '<S1>:257' */
3318 /* 下滑纵摇电子上限位测试 */
3319 /* Entry Internal 'XHZY': '<S1>:257' */
3320 /* Transition: '<S1>:693' */
3321 motor_DWork.is_XHZY_a = motor_IN_Int;
3322 motor_DWork.temporalCounter_i1 = 0U;
3323
3324 /* Entry 'Int': '<S1>:259' */
3325 /* Simulink Function 'Angle_Calculation_XH': '<S1>:250' */
3326 motor_B.Encode_Pos_d = motor_Y.Encode_Pos;
3327
3328 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_XH' */
3329 /* DataTypeConversion: '<S4>/Data Type Conversion2' */
3330 motor_B.DataTypeConversion2 = motor_B.Encode_Pos_d;
3331
3332 /* Gain: '<S4>/GXZ6' */
3333 motor_B.GXZ6 = motor_P.GXZ6_Gain * motor_B.DataTypeConversion2;
3334
3335 /* Gain: '<S4>/GXZ1' */
3336 motor_B.GXZ1 = motor_P.GXZ1_Gain * motor_B.GXZ6;
3337
3338 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_XH' */
3339 motor_DWork.chu_jd = motor_B.GXZ1;
3340 motor_Y.DCZD = false;
3341
3342 /* 解除制动 */
3343 motor_Y.Motor_En = false;
3344
3345 /* 电机使能 */
3346 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
3347 motor_DWork.KG = 1U;
3348 motor_DWork.KG_JD = 0U;
3349 motor_DWork.KG_YJ = 0U;
3350 motor_DWork.KG_En = 1U;
3351 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
3352 } else if (motor_U.Motor_Num == 3) {
3353 /* Transition: '<S1>:665' */
3354 motor_DWork.is_Limit_Up_Test = motor_IN_XHHY_m1;
3355
3356 /* Entry 'XHHY': '<S1>:256' */
3357 /* 下滑横摇电子上限位测试 */
3358 /* Entry Internal 'XHHY': '<S1>:256' */
3359 /* Transition: '<S1>:685' */
3360 motor_DWork.is_XHHY_nl = motor_IN_Int;
3361 motor_DWork.temporalCounter_i1 = 0U;
3362
3363 /* Entry 'Int': '<S1>:268' */
3364 /* Simulink Function 'Angle_Calculation_XH': '<S1>:250' */
3365 motor_B.Encode_Pos_d = motor_Y.Encode_Pos;
3366
3367 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_XH' */
3368 /* DataTypeConversion: '<S4>/Data Type Conversion2' */
3369 motor_B.DataTypeConversion2 = motor_B.Encode_Pos_d;
3370
3371 /* Gain: '<S4>/GXZ6' */
3372 motor_B.GXZ6 = motor_P.GXZ6_Gain * motor_B.DataTypeConversion2;
3373
3374 /* Gain: '<S4>/GXZ1' */
3375 motor_B.GXZ1 = motor_P.GXZ1_Gain * motor_B.GXZ6;
3376
3377 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_XH' */
3378 motor_DWork.chu_jd = motor_B.GXZ1;
3379 motor_Y.DCZD = false;
3380
3381 /* 解除制动 */
3382 motor_Y.Motor_En = false;
3383
3384 /* 电机使能 */
3385 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
3386 motor_DWork.KG = 1U;
3387 motor_DWork.KG_JD = 0U;
3388 motor_DWork.KG_En = 1U;
3389 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
3390 } else {
3391 if (motor_U.Motor_Num == 2) {
3392 /* Transition: '<S1>:667' */
3393 motor_DWork.is_Limit_Up_Test = motor_IN_HY_f;
3394
3395 /* Entry 'HY': '<S1>:258' */
3396 /* 调试模式 */
3397 /* Entry Internal 'HY': '<S1>:258' */
3398 /* Transition: '<S1>:701' */
3399 motor_DWork.is_HY_a = motor_IN_Int;
3400 motor_DWork.temporalCounter_i1 = 0U;
3401
3402 /* Entry 'Int': '<S1>:275' */
3403 /* Simulink Function 'Angle_Calculation_HY': '<S1>:253' */
3404 motor_B.Encode_Pos_dc = motor_Y.Encode_Pos;
3405
3406 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_HY' */
3407 motor_Angle_Calculation_HY(motor_B.Encode_Pos_dc,
3408 &motor_B.Angle_Calculation_HY, (rtP_Angle_Calculation_HY_motor *)
3409 &motor_P.Angle_Calculation_HY);
3410
3411 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_HY' */
3412 motor_DWork.chu_jd = motor_B.Angle_Calculation_HY.GHDG9;
3413 motor_Y.DCZD = false;
3414
3415 /* 解除制动 */
3416 motor_Y.Motor_En = false;
3417
3418 /* 电机使能 */
3419 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
3420 motor_DWork.KG = 1U;
3421 motor_DWork.KG_JD = 0U;
3422 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
3423 }
3424 }
3425 } else if (motor_U.Test_Mode == 3) {
3426 /* Transition: '<S1>:430' */
3427 motor_DWork.is_Test_Mode = motor_IN_Limit_Down_Test;
3428
3429 /* Inport: '<Root>/Motor_Num' */
3430 /* Entry 'Limit_Down_Test': '<S1>:240' */
3431 /* 电子下限位检测 */
3432 /* Entry Internal 'Limit_Down_Test': '<S1>:240' */
3433 /* Transition: '<S1>:675' */
3434 if (motor_U.Motor_Num == 1) {
3435 /* Transition: '<S1>:677' */
3436 motor_DWork.is_Limit_Down_Test = motor_IN_XHZY_pv;
3437
3438 /* Entry 'XHZY': '<S1>:288' */
3439 /* 下滑纵摇电子下限位测试 */
3440 /* Entry Internal 'XHZY': '<S1>:288' */
3441 /* Transition: '<S1>:717' */
3442 motor_DWork.is_XHZY_i = motor_IN_Int;
3443 motor_DWork.temporalCounter_i1 = 0U;
3444
3445 /* Entry 'Int': '<S1>:289' */
3446 /* Simulink Function 'Angle_Calculation_XH': '<S1>:250' */
3447 motor_B.Encode_Pos_d = motor_Y.Encode_Pos;
3448
3449 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_XH' */
3450 /* DataTypeConversion: '<S4>/Data Type Conversion2' */
3451 motor_B.DataTypeConversion2 = motor_B.Encode_Pos_d;
3452
3453 /* Gain: '<S4>/GXZ6' */
3454 motor_B.GXZ6 = motor_P.GXZ6_Gain * motor_B.DataTypeConversion2;
3455
3456 /* Gain: '<S4>/GXZ1' */
3457 motor_B.GXZ1 = motor_P.GXZ1_Gain * motor_B.GXZ6;
3458
3459 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_XH' */
3460 motor_DWork.chu_jd = motor_B.GXZ1;
3461 motor_Y.DCZD = false;
3462
3463 /* 解除制动 */
3464 motor_Y.Motor_En = false;
3465
3466 /* 电机使能 */
3467 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
3468 motor_DWork.KG = 1U;
3469 motor_DWork.KG_JD = 0U;
3470 motor_DWork.KG_YJ = 0U;
3471 motor_DWork.KG_En = 1U;
3472 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
3473 } else if (motor_U.Motor_Num == 3) {
3474 /* Transition: '<S1>:676' */
3475 motor_DWork.is_Limit_Down_Test = motor_IN_XHHY_m1;
3476
3477 /* Entry 'XHHY': '<S1>:296' */
3478 /* 下滑横摇电子下限位测试 */
3479 /* Entry Internal 'XHHY': '<S1>:296' */
3480 /* Transition: '<S1>:725' */
3481 motor_DWork.is_XHHY_f = motor_IN_Int;
3482 motor_DWork.temporalCounter_i1 = 0U;
3483
3484 /* Entry 'Int': '<S1>:297' */
3485 /* Simulink Function 'Angle_Calculation_XH': '<S1>:250' */
3486 motor_B.Encode_Pos_d = motor_Y.Encode_Pos;
3487
3488 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_XH' */
3489 /* DataTypeConversion: '<S4>/Data Type Conversion2' */
3490 motor_B.DataTypeConversion2 = motor_B.Encode_Pos_d;
3491
3492 /* Gain: '<S4>/GXZ6' */
3493 motor_B.GXZ6 = motor_P.GXZ6_Gain * motor_B.DataTypeConversion2;
3494
3495 /* Gain: '<S4>/GXZ1' */
3496 motor_B.GXZ1 = motor_P.GXZ1_Gain * motor_B.GXZ6;
3497
3498 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_XH' */
3499 motor_DWork.chu_jd = motor_B.GXZ1;
3500 motor_Y.DCZD = false;
3501
3502 /* 解除制动 */
3503 motor_Y.Motor_En = false;
3504
3505 /* 电机使能 */
3506 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
3507 motor_DWork.KG = 1U;
3508 motor_DWork.KG_JD = 0U;
3509 motor_DWork.KG_En = 1U;
3510 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
3511 } else {
3512 if (motor_U.Motor_Num == 2) {
3513 /* Transition: '<S1>:678' */
3514 motor_DWork.is_Limit_Down_Test = motor_IN_HY_f;
3515
3516 /* Entry 'HY': '<S1>:280' */
3517 /* 横摇灯杆电子下限位测试 */
3518 /* Entry Internal 'HY': '<S1>:280' */
3519 /* Transition: '<S1>:709' */
3520 motor_DWork.is_HY_h = motor_IN_Int;
3521 motor_DWork.temporalCounter_i1 = 0U;
3522
3523 /* Entry 'Int': '<S1>:281' */
3524 /* Simulink Function 'Angle_Calculation_HY': '<S1>:253' */
3525 motor_B.Encode_Pos_dc = motor_Y.Encode_Pos;
3526
3527 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_HY' */
3528 motor_Angle_Calculation_HY(motor_B.Encode_Pos_dc,
3529 &motor_B.Angle_Calculation_HY, (rtP_Angle_Calculation_HY_motor *)
3530 &motor_P.Angle_Calculation_HY);
3531
3532 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_HY' */
3533 motor_DWork.chu_jd = motor_B.Angle_Calculation_HY.GHDG9;
3534 motor_Y.DCZD = false;
3535
3536 /* 解除制动 */
3537 motor_Y.Motor_En = false;
3538
3539 /* 电机使能 */
3540 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
3541 motor_DWork.KG = 1U;
3542 motor_DWork.KG_JD = 0U;
3543 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
3544 }
3545 }
3546 } else if ((motor_U.Test_Mode == 4) && (motor_U.Motor_Num == 1)) {
3547 /* Transition: '<S1>:427' */
3548 motor_DWork.is_Test_Mode = motor_IN_Elevation_Test;
3549
3550 /* Entry 'Elevation_Test': '<S1>:304' */
3551 /* 标定模式 */
3552 motor_Y.DCZD = false;
3553
3554 /* 解除制动 */
3555 motor_Y.Motor_En = false;
3556
3557 /* 电机使能 */
3558 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
3559 motor_DWork.chu_jd = 0.0;
3560
3561 /* Entry Internal 'Elevation_Test': '<S1>:304' */
3562 /* Transition: '<S1>:734' */
3563 motor_DWork.is_Elevation_Test = motor_IN_Int;
3564 motor_DWork.temporalCounter_i1 = 0U;
3565
3566 /* Entry 'Int': '<S1>:305' */
3567 motor_DWork.KG = 1U;
3568 motor_DWork.KG_JD = 0U;
3569 motor_DWork.KG_YJ = 1U;
3570 motor_DWork.KG_En = 1U;
3571 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
3572 } else {
3573 if (motor_U.Test_Mode == 5) {
3574 /* Transition: '<S1>:1064' */
3575 motor_DWork.is_Test_Mode = motor_IN_Dspace_Test;
3576
3577 /* Entry 'Dspace_Test': '<S1>:1051' */
3578 /* Dspace调试 */
3579 motor_Y.DCZD = false;
3580
3581 /* 解除制动 */
3582 motor_Y.Motor_En = false;
3583
3584 /* 电机使能 */
3585 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
3586 motor_DWork.chu_jd = 0.0;
3587
3588 /* Entry Internal 'Dspace_Test': '<S1>:1051' */
3589 /* Transition: '<S1>:1054' */
3590 motor_DWork.is_Dspace_Test = motor_IN_Int;
3591 motor_DWork.temporalCounter_i1 = 0U;
3592
3593 /* Entry 'Int': '<S1>:1058' */
3594 motor_DWork.KG = 1U;
3595 motor_DWork.KG_JD = 0U;
3596 motor_DWork.KG_YJ = 0U;
3597 motor_DWork.KG_En = 1U;
3598 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
3599 }
3600 }
3601
3602 /* End of Inport: '<Root>/Test_Mode' */
3603}
3604
3605/* Function for Chart: '<Root>/Chart' */
3606static void motor_XHZY_as(void)
3607{
3608 int32_T tmp;
3609 uint16_T tmp_0;
3610 real_T tmp_1;
3611
3612 /* During 'XHZY': '<S1>:288' */
3613 switch (motor_DWork.is_XHZY_i) {
3614 case motor_IN_Int:
3615 /* During 'Int': '<S1>:289' */
3616 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
3617 /* Transition: '<S1>:718' */
3618 motor_DWork.is_XHZY_i = motor_IN_Int8;
3619 }
3620 break;
3621
3622 case motor_IN_Int1:
3623 /* Inport: '<Root>/Down_Limit' */
3624 /* During 'Int1': '<S1>:290' */
3625 if (motor_U.Down_Limit == 0) {
3626 /* Transition: '<S1>:719' */
3627 /* 下限位开关低电平有效 */
3628 motor_DWork.is_XHZY_i = motor_IN_Int2_i;
3629 motor_DWork.temporalCounter_i1 = 0U;
3630
3631 /* Entry 'Int2': '<S1>:291' */
3632 motor_DWork.chu_jd -= 0.002;
3633
3634 /* Inport: '<Root>/JD_In' */
3635 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
3636 motor_B.JD_In_d = motor_U.JD_In;
3637 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
3638
3639 /* Inport: '<Root>/YJ_In' */
3640 motor_B.YJ_In = motor_U.YJ_In;
3641
3642 /* Inport: '<Root>/Encode_Sp' */
3643 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
3644
3645 /* Inport: '<Root>/System_Order' */
3646 motor_B.Slect_port_h = motor_U.System_Order;
3647
3648 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
3649 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
3650 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
3651 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
3652 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
3653 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
3654 &motor_DWork.EN_extern, &motor_DWork.Forword,
3655 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
3656 &motor_DWork.KG_En, &motor_DWork.KG_JD,
3657 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
3658 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
3659 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
3660 &motor_DWork.chu_jd);
3661
3662 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
3663 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
3664 if (tmp_1 < 65536.0) {
3665 if (tmp_1 >= 0.0) {
3666 tmp_0 = (uint16_T)tmp_1;
3667 } else {
3668 tmp_0 = 0U;
3669 }
3670 } else {
3671 tmp_0 = MAX_uint16_T;
3672 }
3673
3674 motor_Y.PWMOUT = tmp_0;
3675 tmp_1 = motor_B.Motor_XHZY.KP_JD;
3676 if (tmp_1 < 2.147483648E+9) {
3677 if (tmp_1 >= -2.147483648E+9) {
3678 tmp = (int32_T)tmp_1;
3679 } else {
3680 tmp = MIN_int32_T;
3681 }
3682 } else {
3683 tmp = MAX_int32_T;
3684 }
3685
3686 motor_Y.JD_Out = tmp;
3687 tmp_1 = motor_B.Motor_XHZY.KP_e;
3688 if (tmp_1 < 2.147483648E+9) {
3689 if (tmp_1 >= -2.147483648E+9) {
3690 tmp = (int32_T)tmp_1;
3691 } else {
3692 tmp = MIN_int32_T;
3693 }
3694 } else {
3695 tmp = MAX_int32_T;
3696 }
3697
3698 motor_Y.JD_Error = tmp;
3699 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
3700 } else if (motor_DWork.temporalCounter_i1 >= (uint32_T)(5.0 / motor_DWork.Ts))
3701 {
3702 /* Transition: '<S1>:1039' */
3703 motor_Y.Flag_Down_GZ_limit = 0U;
3704
3705 /* 下限位开关故障$/ */
3706 motor_DWork.is_XHZY_i = motor_IN_Int2_i;
3707 motor_DWork.temporalCounter_i1 = 0U;
3708
3709 /* Entry 'Int2': '<S1>:291' */
3710 motor_DWork.chu_jd -= 0.002;
3711
3712 /* Inport: '<Root>/JD_In' */
3713 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
3714 motor_B.JD_In_d = motor_U.JD_In;
3715 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
3716
3717 /* Inport: '<Root>/YJ_In' */
3718 motor_B.YJ_In = motor_U.YJ_In;
3719
3720 /* Inport: '<Root>/Encode_Sp' */
3721 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
3722
3723 /* Inport: '<Root>/System_Order' */
3724 motor_B.Slect_port_h = motor_U.System_Order;
3725
3726 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
3727 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
3728 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
3729 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
3730 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
3731 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
3732 &motor_DWork.EN_extern, &motor_DWork.Forword,
3733 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
3734 &motor_DWork.KG_En, &motor_DWork.KG_JD,
3735 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
3736 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
3737 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
3738 &motor_DWork.chu_jd);
3739
3740 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
3741 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
3742 if (tmp_1 < 65536.0) {
3743 if (tmp_1 >= 0.0) {
3744 tmp_0 = (uint16_T)tmp_1;
3745 } else {
3746 tmp_0 = 0U;
3747 }
3748 } else {
3749 tmp_0 = MAX_uint16_T;
3750 }
3751
3752 motor_Y.PWMOUT = tmp_0;
3753 tmp_1 = motor_B.Motor_XHZY.KP_JD;
3754 if (tmp_1 < 2.147483648E+9) {
3755 if (tmp_1 >= -2.147483648E+9) {
3756 tmp = (int32_T)tmp_1;
3757 } else {
3758 tmp = MIN_int32_T;
3759 }
3760 } else {
3761 tmp = MAX_int32_T;
3762 }
3763
3764 motor_Y.JD_Out = tmp;
3765 tmp_1 = motor_B.Motor_XHZY.KP_e;
3766 if (tmp_1 < 2.147483648E+9) {
3767 if (tmp_1 >= -2.147483648E+9) {
3768 tmp = (int32_T)tmp_1;
3769 } else {
3770 tmp = MIN_int32_T;
3771 }
3772 } else {
3773 tmp = MAX_int32_T;
3774 }
3775
3776 motor_Y.JD_Error = tmp;
3777 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
3778 } else {
3779 motor_DWork.chu_jd -= 0.002;
3780
3781 /* Inport: '<Root>/JD_In' */
3782 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
3783 motor_B.JD_In_d = motor_U.JD_In;
3784 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
3785
3786 /* Inport: '<Root>/YJ_In' */
3787 motor_B.YJ_In = motor_U.YJ_In;
3788
3789 /* Inport: '<Root>/Encode_Sp' */
3790 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
3791
3792 /* Inport: '<Root>/System_Order' */
3793 motor_B.Slect_port_h = motor_U.System_Order;
3794
3795 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
3796 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
3797 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
3798 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
3799 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
3800 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
3801 &motor_DWork.EN_extern, &motor_DWork.Forword,
3802 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
3803 &motor_DWork.KG_En, &motor_DWork.KG_JD,
3804 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
3805 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
3806 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
3807 &motor_DWork.chu_jd);
3808
3809 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
3810 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
3811 if (tmp_1 < 65536.0) {
3812 if (tmp_1 >= 0.0) {
3813 tmp_0 = (uint16_T)tmp_1;
3814 } else {
3815 tmp_0 = 0U;
3816 }
3817 } else {
3818 tmp_0 = MAX_uint16_T;
3819 }
3820
3821 motor_Y.PWMOUT = tmp_0;
3822 tmp_1 = motor_B.Motor_XHZY.KP_JD;
3823 if (tmp_1 < 2.147483648E+9) {
3824 if (tmp_1 >= -2.147483648E+9) {
3825 tmp = (int32_T)tmp_1;
3826 } else {
3827 tmp = MIN_int32_T;
3828 }
3829 } else {
3830 tmp = MAX_int32_T;
3831 }
3832
3833 motor_Y.JD_Out = tmp;
3834 tmp_1 = motor_B.Motor_XHZY.KP_e;
3835 if (tmp_1 < 2.147483648E+9) {
3836 if (tmp_1 >= -2.147483648E+9) {
3837 tmp = (int32_T)tmp_1;
3838 } else {
3839 tmp = MIN_int32_T;
3840 }
3841 } else {
3842 tmp = MAX_int32_T;
3843 }
3844
3845 motor_Y.JD_Error = tmp;
3846 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
3847 }
3848
3849 /* End of Inport: '<Root>/Down_Limit' */
3850 break;
3851
3852 case motor_IN_Int2_i:
3853 /* During 'Int2': '<S1>:291' */
3854 if (motor_DWork.temporalCounter_i1 >= 50U) {
3855 /* Transition: '<S1>:721' */
3856 motor_DWork.is_XHZY_i = motor_IN_Int7;
3857 motor_DWork.temporalCounter_i1 = 0U;
3858 } else {
3859 motor_DWork.chu_jd -= 0.002;
3860
3861 /* Inport: '<Root>/JD_In' */
3862 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
3863 motor_B.JD_In_d = motor_U.JD_In;
3864 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
3865
3866 /* Inport: '<Root>/YJ_In' */
3867 motor_B.YJ_In = motor_U.YJ_In;
3868
3869 /* Inport: '<Root>/Encode_Sp' */
3870 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
3871
3872 /* Inport: '<Root>/System_Order' */
3873 motor_B.Slect_port_h = motor_U.System_Order;
3874
3875 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
3876 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
3877 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
3878 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
3879 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
3880 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
3881 &motor_DWork.EN_extern, &motor_DWork.Forword,
3882 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
3883 &motor_DWork.KG_En, &motor_DWork.KG_JD,
3884 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
3885 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
3886 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
3887 &motor_DWork.chu_jd);
3888
3889 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
3890 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
3891 if (tmp_1 < 65536.0) {
3892 if (tmp_1 >= 0.0) {
3893 tmp_0 = (uint16_T)tmp_1;
3894 } else {
3895 tmp_0 = 0U;
3896 }
3897 } else {
3898 tmp_0 = MAX_uint16_T;
3899 }
3900
3901 motor_Y.PWMOUT = tmp_0;
3902 tmp_1 = motor_B.Motor_XHZY.KP_JD;
3903 if (tmp_1 < 2.147483648E+9) {
3904 if (tmp_1 >= -2.147483648E+9) {
3905 tmp = (int32_T)tmp_1;
3906 } else {
3907 tmp = MIN_int32_T;
3908 }
3909 } else {
3910 tmp = MAX_int32_T;
3911 }
3912
3913 motor_Y.JD_Out = tmp;
3914 tmp_1 = motor_B.Motor_XHZY.KP_e;
3915 if (tmp_1 < 2.147483648E+9) {
3916 if (tmp_1 >= -2.147483648E+9) {
3917 tmp = (int32_T)tmp_1;
3918 } else {
3919 tmp = MIN_int32_T;
3920 }
3921 } else {
3922 tmp = MAX_int32_T;
3923 }
3924
3925 motor_Y.JD_Error = tmp;
3926 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
3927 }
3928 break;
3929
3930 case motor_IN_Int3_l:
3931 /* During 'Int3': '<S1>:292' */
3932 if (fabs(motor_DWork.chu_jd) < 0.1) {
3933 /* Transition: '<S1>:723' */
3934 motor_DWork.is_XHZY_i = motor_IN_Int4_b;
3935 motor_DWork.temporalCounter_i1 = 0U;
3936
3937 /* Entry 'Int4': '<S1>:294' */
3938 motor_DWork.chu_jd = 0.0;
3939
3940 /* Inport: '<Root>/JD_In' */
3941 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
3942 motor_B.JD_In_d = motor_U.JD_In;
3943 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
3944
3945 /* Inport: '<Root>/YJ_In' */
3946 motor_B.YJ_In = motor_U.YJ_In;
3947
3948 /* Inport: '<Root>/Encode_Sp' */
3949 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
3950
3951 /* Inport: '<Root>/System_Order' */
3952 motor_B.Slect_port_h = motor_U.System_Order;
3953
3954 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
3955 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
3956 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
3957 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
3958 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
3959 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
3960 &motor_DWork.EN_extern, &motor_DWork.Forword,
3961 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
3962 &motor_DWork.KG_En, &motor_DWork.KG_JD,
3963 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
3964 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
3965 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
3966 &motor_DWork.chu_jd);
3967
3968 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
3969 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
3970 if (tmp_1 < 65536.0) {
3971 if (tmp_1 >= 0.0) {
3972 tmp_0 = (uint16_T)tmp_1;
3973 } else {
3974 tmp_0 = 0U;
3975 }
3976 } else {
3977 tmp_0 = MAX_uint16_T;
3978 }
3979
3980 motor_Y.PWMOUT = tmp_0;
3981 tmp_1 = motor_B.Motor_XHZY.KP_JD;
3982 if (tmp_1 < 2.147483648E+9) {
3983 if (tmp_1 >= -2.147483648E+9) {
3984 tmp = (int32_T)tmp_1;
3985 } else {
3986 tmp = MIN_int32_T;
3987 }
3988 } else {
3989 tmp = MAX_int32_T;
3990 }
3991
3992 motor_Y.JD_Out = tmp;
3993 tmp_1 = motor_B.Motor_XHZY.KP_e;
3994 if (tmp_1 < 2.147483648E+9) {
3995 if (tmp_1 >= -2.147483648E+9) {
3996 tmp = (int32_T)tmp_1;
3997 } else {
3998 tmp = MIN_int32_T;
3999 }
4000 } else {
4001 tmp = MAX_int32_T;
4002 }
4003
4004 motor_Y.JD_Error = tmp;
4005 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
4006 } else {
4007 motor_DWork.chu_jd += 0.01;
4008
4009 /* Inport: '<Root>/JD_In' */
4010 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
4011 motor_B.JD_In_d = motor_U.JD_In;
4012 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
4013
4014 /* Inport: '<Root>/YJ_In' */
4015 motor_B.YJ_In = motor_U.YJ_In;
4016
4017 /* Inport: '<Root>/Encode_Sp' */
4018 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
4019
4020 /* Inport: '<Root>/System_Order' */
4021 motor_B.Slect_port_h = motor_U.System_Order;
4022
4023 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
4024 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
4025 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
4026 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
4027 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
4028 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
4029 &motor_DWork.EN_extern, &motor_DWork.Forword,
4030 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
4031 &motor_DWork.KG_En, &motor_DWork.KG_JD,
4032 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
4033 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
4034 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
4035 &motor_DWork.chu_jd);
4036
4037 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
4038 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
4039 if (tmp_1 < 65536.0) {
4040 if (tmp_1 >= 0.0) {
4041 tmp_0 = (uint16_T)tmp_1;
4042 } else {
4043 tmp_0 = 0U;
4044 }
4045 } else {
4046 tmp_0 = MAX_uint16_T;
4047 }
4048
4049 motor_Y.PWMOUT = tmp_0;
4050 tmp_1 = motor_B.Motor_XHZY.KP_JD;
4051 if (tmp_1 < 2.147483648E+9) {
4052 if (tmp_1 >= -2.147483648E+9) {
4053 tmp = (int32_T)tmp_1;
4054 } else {
4055 tmp = MIN_int32_T;
4056 }
4057 } else {
4058 tmp = MAX_int32_T;
4059 }
4060
4061 motor_Y.JD_Out = tmp;
4062 tmp_1 = motor_B.Motor_XHZY.KP_e;
4063 if (tmp_1 < 2.147483648E+9) {
4064 if (tmp_1 >= -2.147483648E+9) {
4065 tmp = (int32_T)tmp_1;
4066 } else {
4067 tmp = MIN_int32_T;
4068 }
4069 } else {
4070 tmp = MAX_int32_T;
4071 }
4072
4073 motor_Y.JD_Error = tmp;
4074 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
4075 }
4076 break;
4077
4078 case motor_IN_Int4_b:
4079 /* During 'Int4': '<S1>:294' */
4080 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
4081 /* Transition: '<S1>:724' */
4082 motor_DWork.is_XHZY_i = motor_IN_Int5_a;
4083 motor_DWork.temporalCounter_i1 = 0U;
4084
4085 /* Entry 'Int5': '<S1>:295' */
4086 motor_Y.Motor_En = true;
4087
4088 /* 电机失能 */
4089 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
4090 } else {
4091 /* Inport: '<Root>/JD_In' */
4092 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
4093 motor_B.JD_In_d = motor_U.JD_In;
4094 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
4095
4096 /* Inport: '<Root>/YJ_In' */
4097 motor_B.YJ_In = motor_U.YJ_In;
4098
4099 /* Inport: '<Root>/Encode_Sp' */
4100 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
4101
4102 /* Inport: '<Root>/System_Order' */
4103 motor_B.Slect_port_h = motor_U.System_Order;
4104
4105 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
4106 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
4107 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
4108 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
4109 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
4110 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
4111 &motor_DWork.EN_extern, &motor_DWork.Forword,
4112 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
4113 &motor_DWork.KG_En, &motor_DWork.KG_JD,
4114 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
4115 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
4116 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
4117 &motor_DWork.chu_jd);
4118
4119 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
4120 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
4121 if (tmp_1 < 65536.0) {
4122 if (tmp_1 >= 0.0) {
4123 tmp_0 = (uint16_T)tmp_1;
4124 } else {
4125 tmp_0 = 0U;
4126 }
4127 } else {
4128 tmp_0 = MAX_uint16_T;
4129 }
4130
4131 motor_Y.PWMOUT = tmp_0;
4132 tmp_1 = motor_B.Motor_XHZY.KP_JD;
4133 if (tmp_1 < 2.147483648E+9) {
4134 if (tmp_1 >= -2.147483648E+9) {
4135 tmp = (int32_T)tmp_1;
4136 } else {
4137 tmp = MIN_int32_T;
4138 }
4139 } else {
4140 tmp = MAX_int32_T;
4141 }
4142
4143 motor_Y.JD_Out = tmp;
4144 tmp_1 = motor_B.Motor_XHZY.KP_e;
4145 if (tmp_1 < 2.147483648E+9) {
4146 if (tmp_1 >= -2.147483648E+9) {
4147 tmp = (int32_T)tmp_1;
4148 } else {
4149 tmp = MIN_int32_T;
4150 }
4151 } else {
4152 tmp = MAX_int32_T;
4153 }
4154
4155 motor_Y.JD_Error = tmp;
4156 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
4157 }
4158 break;
4159
4160 case motor_IN_Int5_a:
4161 /* During 'Int5': '<S1>:295' */
4162 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
4163 /* Transition: '<S1>:722' */
4164 motor_DWork.is_XHZY_i = motor_IN_Int6_m;
4165
4166 /* Entry 'Int6': '<S1>:293' */
4167 motor_Y.DCZD = true;
4168
4169 /* 开启制动 */
4170 }
4171 break;
4172
4173 case motor_IN_Int6_m:
4174 /* During 'Int6': '<S1>:293' */
4175 /* Transition: '<S1>:720' */
4176 /* Transition: '<S1>:674' */
4177 motor_DWork.is_XHZY_i = motor_IN_NO_ACTIVE_CHILD;
4178 motor_DWork.is_Limit_Down_Test = motor_IN_NO_ACTIVE_CHILD;
4179 motor_DWork.is_Test_Mode = motor_IN_defult;
4180
4181 /* Entry 'defult': '<S1>:239' */
4182 motor_Y.Open_Result = 1U;
4183
4184 /* 开机状态位成功 */
4185 motor_DWork.In_State = 2U;
4186 break;
4187
4188 case motor_IN_Int7:
4189 /* During 'Int7': '<S1>:973' */
4190 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
4191 /* Transition: '<S1>:974' */
4192 motor_DWork.is_XHZY_i = motor_IN_Int3_l;
4193
4194 /* Entry 'Int3': '<S1>:292' */
4195 motor_DWork.chu_jd += 0.01;
4196
4197 /* Inport: '<Root>/JD_In' */
4198 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
4199 motor_B.JD_In_d = motor_U.JD_In;
4200 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
4201
4202 /* Inport: '<Root>/YJ_In' */
4203 motor_B.YJ_In = motor_U.YJ_In;
4204
4205 /* Inport: '<Root>/Encode_Sp' */
4206 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
4207
4208 /* Inport: '<Root>/System_Order' */
4209 motor_B.Slect_port_h = motor_U.System_Order;
4210
4211 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
4212 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
4213 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
4214 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
4215 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
4216 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
4217 &motor_DWork.EN_extern, &motor_DWork.Forword,
4218 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
4219 &motor_DWork.KG_En, &motor_DWork.KG_JD,
4220 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
4221 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
4222 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
4223 &motor_DWork.chu_jd);
4224
4225 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
4226 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
4227 if (tmp_1 < 65536.0) {
4228 if (tmp_1 >= 0.0) {
4229 tmp_0 = (uint16_T)tmp_1;
4230 } else {
4231 tmp_0 = 0U;
4232 }
4233 } else {
4234 tmp_0 = MAX_uint16_T;
4235 }
4236
4237 motor_Y.PWMOUT = tmp_0;
4238 tmp_1 = motor_B.Motor_XHZY.KP_JD;
4239 if (tmp_1 < 2.147483648E+9) {
4240 if (tmp_1 >= -2.147483648E+9) {
4241 tmp = (int32_T)tmp_1;
4242 } else {
4243 tmp = MIN_int32_T;
4244 }
4245 } else {
4246 tmp = MAX_int32_T;
4247 }
4248
4249 motor_Y.JD_Out = tmp;
4250 tmp_1 = motor_B.Motor_XHZY.KP_e;
4251 if (tmp_1 < 2.147483648E+9) {
4252 if (tmp_1 >= -2.147483648E+9) {
4253 tmp = (int32_T)tmp_1;
4254 } else {
4255 tmp = MIN_int32_T;
4256 }
4257 } else {
4258 tmp = MAX_int32_T;
4259 }
4260
4261 motor_Y.JD_Error = tmp;
4262 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
4263 } else {
4264 /* Inport: '<Root>/JD_In' */
4265 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
4266 motor_B.JD_In_d = motor_U.JD_In;
4267 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
4268
4269 /* Inport: '<Root>/YJ_In' */
4270 motor_B.YJ_In = motor_U.YJ_In;
4271
4272 /* Inport: '<Root>/Encode_Sp' */
4273 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
4274
4275 /* Inport: '<Root>/System_Order' */
4276 motor_B.Slect_port_h = motor_U.System_Order;
4277
4278 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
4279 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
4280 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
4281 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
4282 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
4283 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
4284 &motor_DWork.EN_extern, &motor_DWork.Forword,
4285 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
4286 &motor_DWork.KG_En, &motor_DWork.KG_JD,
4287 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
4288 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
4289 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
4290 &motor_DWork.chu_jd);
4291
4292 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
4293 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
4294 if (tmp_1 < 65536.0) {
4295 if (tmp_1 >= 0.0) {
4296 tmp_0 = (uint16_T)tmp_1;
4297 } else {
4298 tmp_0 = 0U;
4299 }
4300 } else {
4301 tmp_0 = MAX_uint16_T;
4302 }
4303
4304 motor_Y.PWMOUT = tmp_0;
4305 tmp_1 = motor_B.Motor_XHZY.KP_JD;
4306 if (tmp_1 < 2.147483648E+9) {
4307 if (tmp_1 >= -2.147483648E+9) {
4308 tmp = (int32_T)tmp_1;
4309 } else {
4310 tmp = MIN_int32_T;
4311 }
4312 } else {
4313 tmp = MAX_int32_T;
4314 }
4315
4316 motor_Y.JD_Out = tmp;
4317 tmp_1 = motor_B.Motor_XHZY.KP_e;
4318 if (tmp_1 < 2.147483648E+9) {
4319 if (tmp_1 >= -2.147483648E+9) {
4320 tmp = (int32_T)tmp_1;
4321 } else {
4322 tmp = MIN_int32_T;
4323 }
4324 } else {
4325 tmp = MAX_int32_T;
4326 }
4327
4328 motor_Y.JD_Error = tmp;
4329 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
4330 }
4331 break;
4332
4333 default:
4334 /* During 'Int8': '<S1>:992' */
4335 if (motor_DWork.chu_jd < -16.0) {
4336 /* Transition: '<S1>:993' */
4337 motor_DWork.is_XHZY_i = motor_IN_Int1;
4338 motor_DWork.temporalCounter_i1 = 0U;
4339
4340 /* Entry 'Int1': '<S1>:290' */
4341 motor_DWork.chu_jd -= 0.002;
4342
4343 /* Inport: '<Root>/JD_In' */
4344 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
4345 motor_B.JD_In_d = motor_U.JD_In;
4346 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
4347
4348 /* Inport: '<Root>/YJ_In' */
4349 motor_B.YJ_In = motor_U.YJ_In;
4350
4351 /* Inport: '<Root>/Encode_Sp' */
4352 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
4353
4354 /* Inport: '<Root>/System_Order' */
4355 motor_B.Slect_port_h = motor_U.System_Order;
4356
4357 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
4358 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
4359 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
4360 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
4361 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
4362 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
4363 &motor_DWork.EN_extern, &motor_DWork.Forword,
4364 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
4365 &motor_DWork.KG_En, &motor_DWork.KG_JD,
4366 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
4367 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
4368 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
4369 &motor_DWork.chu_jd);
4370
4371 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
4372 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
4373 if (tmp_1 < 65536.0) {
4374 if (tmp_1 >= 0.0) {
4375 tmp_0 = (uint16_T)tmp_1;
4376 } else {
4377 tmp_0 = 0U;
4378 }
4379 } else {
4380 tmp_0 = MAX_uint16_T;
4381 }
4382
4383 motor_Y.PWMOUT = tmp_0;
4384 tmp_1 = motor_B.Motor_XHZY.KP_JD;
4385 if (tmp_1 < 2.147483648E+9) {
4386 if (tmp_1 >= -2.147483648E+9) {
4387 tmp = (int32_T)tmp_1;
4388 } else {
4389 tmp = MIN_int32_T;
4390 }
4391 } else {
4392 tmp = MAX_int32_T;
4393 }
4394
4395 motor_Y.JD_Out = tmp;
4396 tmp_1 = motor_B.Motor_XHZY.KP_e;
4397 if (tmp_1 < 2.147483648E+9) {
4398 if (tmp_1 >= -2.147483648E+9) {
4399 tmp = (int32_T)tmp_1;
4400 } else {
4401 tmp = MIN_int32_T;
4402 }
4403 } else {
4404 tmp = MAX_int32_T;
4405 }
4406
4407 motor_Y.JD_Error = tmp;
4408 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
4409 } else if (motor_U.Down_Limit == 0) {
4410 /* Transition: '<S1>:994' */
4411 /* 下限位开关低电平有效 */
4412 motor_DWork.is_XHZY_i = motor_IN_Int2_i;
4413 motor_DWork.temporalCounter_i1 = 0U;
4414
4415 /* Entry 'Int2': '<S1>:291' */
4416 motor_DWork.chu_jd -= 0.002;
4417
4418 /* Inport: '<Root>/JD_In' */
4419 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
4420 motor_B.JD_In_d = motor_U.JD_In;
4421 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
4422
4423 /* Inport: '<Root>/YJ_In' */
4424 motor_B.YJ_In = motor_U.YJ_In;
4425
4426 /* Inport: '<Root>/Encode_Sp' */
4427 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
4428
4429 /* Inport: '<Root>/System_Order' */
4430 motor_B.Slect_port_h = motor_U.System_Order;
4431
4432 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
4433 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
4434 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
4435 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
4436 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
4437 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
4438 &motor_DWork.EN_extern, &motor_DWork.Forword,
4439 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
4440 &motor_DWork.KG_En, &motor_DWork.KG_JD,
4441 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
4442 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
4443 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
4444 &motor_DWork.chu_jd);
4445
4446 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
4447 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
4448 if (tmp_1 < 65536.0) {
4449 if (tmp_1 >= 0.0) {
4450 tmp_0 = (uint16_T)tmp_1;
4451 } else {
4452 tmp_0 = 0U;
4453 }
4454 } else {
4455 tmp_0 = MAX_uint16_T;
4456 }
4457
4458 motor_Y.PWMOUT = tmp_0;
4459 tmp_1 = motor_B.Motor_XHZY.KP_JD;
4460 if (tmp_1 < 2.147483648E+9) {
4461 if (tmp_1 >= -2.147483648E+9) {
4462 tmp = (int32_T)tmp_1;
4463 } else {
4464 tmp = MIN_int32_T;
4465 }
4466 } else {
4467 tmp = MAX_int32_T;
4468 }
4469
4470 motor_Y.JD_Out = tmp;
4471 tmp_1 = motor_B.Motor_XHZY.KP_e;
4472 if (tmp_1 < 2.147483648E+9) {
4473 if (tmp_1 >= -2.147483648E+9) {
4474 tmp = (int32_T)tmp_1;
4475 } else {
4476 tmp = MIN_int32_T;
4477 }
4478 } else {
4479 tmp = MAX_int32_T;
4480 }
4481
4482 motor_Y.JD_Error = tmp;
4483 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
4484 } else {
4485 motor_DWork.chu_jd -= 0.01;
4486
4487 /* Inport: '<Root>/JD_In' */
4488 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
4489 motor_B.JD_In_d = motor_U.JD_In;
4490 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
4491
4492 /* Inport: '<Root>/YJ_In' */
4493 motor_B.YJ_In = motor_U.YJ_In;
4494
4495 /* Inport: '<Root>/Encode_Sp' */
4496 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
4497
4498 /* Inport: '<Root>/System_Order' */
4499 motor_B.Slect_port_h = motor_U.System_Order;
4500
4501 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
4502 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
4503 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
4504 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
4505 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
4506 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
4507 &motor_DWork.EN_extern, &motor_DWork.Forword,
4508 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
4509 &motor_DWork.KG_En, &motor_DWork.KG_JD,
4510 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
4511 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
4512 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
4513 &motor_DWork.chu_jd);
4514
4515 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
4516 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
4517 if (tmp_1 < 65536.0) {
4518 if (tmp_1 >= 0.0) {
4519 tmp_0 = (uint16_T)tmp_1;
4520 } else {
4521 tmp_0 = 0U;
4522 }
4523 } else {
4524 tmp_0 = MAX_uint16_T;
4525 }
4526
4527 motor_Y.PWMOUT = tmp_0;
4528 tmp_1 = motor_B.Motor_XHZY.KP_JD;
4529 if (tmp_1 < 2.147483648E+9) {
4530 if (tmp_1 >= -2.147483648E+9) {
4531 tmp = (int32_T)tmp_1;
4532 } else {
4533 tmp = MIN_int32_T;
4534 }
4535 } else {
4536 tmp = MAX_int32_T;
4537 }
4538
4539 motor_Y.JD_Out = tmp;
4540 tmp_1 = motor_B.Motor_XHZY.KP_e;
4541 if (tmp_1 < 2.147483648E+9) {
4542 if (tmp_1 >= -2.147483648E+9) {
4543 tmp = (int32_T)tmp_1;
4544 } else {
4545 tmp = MIN_int32_T;
4546 }
4547 } else {
4548 tmp = MAX_int32_T;
4549 }
4550
4551 motor_Y.JD_Error = tmp;
4552 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
4553 }
4554 break;
4555 }
4556}
4557
4558/* Function for Chart: '<Root>/Chart' */
4559static void motor_Limit_Down_Test(void)
4560{
4561 int32_T tmp;
4562 real_T tmp_0;
4563 uint16_T tmp_1;
4564
4565 /* During 'Limit_Down_Test': '<S1>:240' */
4566 switch (motor_DWork.is_Limit_Down_Test) {
4567 case motor_IN_HY_f:
4568 /* During 'HY': '<S1>:280' */
4569 switch (motor_DWork.is_HY_h) {
4570 case motor_IN_Int:
4571 /* During 'Int': '<S1>:281' */
4572 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
4573 /* Transition: '<S1>:710' */
4574 motor_DWork.is_HY_h = motor_IN_Int8;
4575 }
4576 break;
4577
4578 case motor_IN_Int1:
4579 /* Inport: '<Root>/Down_Limit' */
4580 /* During 'Int1': '<S1>:282' */
4581 if (motor_U.Down_Limit == 0) {
4582 /* Transition: '<S1>:711' */
4583 /* 上限位开关低电平有效 */
4584 motor_DWork.is_HY_h = motor_IN_Int2_i;
4585 motor_DWork.temporalCounter_i1 = 0U;
4586
4587 /* Entry 'Int2': '<S1>:283' */
4588 motor_DWork.chu_jd -= 0.002;
4589
4590 /* Inport: '<Root>/JD_In' */
4591 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
4592 motor_B.JD_In_f = motor_U.JD_In;
4593 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
4594
4595 /* Inport: '<Root>/System_Order' */
4596 motor_B.Slect_port_c = motor_U.System_Order;
4597
4598 /* Inport: '<Root>/Encode_Sp' */
4599 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
4600
4601 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
4602 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
4603 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
4604 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
4605 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
4606 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
4607 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
4608 &motor_DWork.chu_jd);
4609
4610 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
4611 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
4612 tmp_0 = motor_B.Motor_HYDG.KP_e;
4613 if (tmp_0 < 2.147483648E+9) {
4614 if (tmp_0 >= -2.147483648E+9) {
4615 tmp = (int32_T)tmp_0;
4616 } else {
4617 tmp = MIN_int32_T;
4618 }
4619 } else {
4620 tmp = MAX_int32_T;
4621 }
4622
4623 motor_Y.JD_Error = tmp;
4624 tmp_0 = motor_B.Motor_HYDG.KP_JD;
4625 if (tmp_0 < 2.147483648E+9) {
4626 if (tmp_0 >= -2.147483648E+9) {
4627 tmp = (int32_T)tmp_0;
4628 } else {
4629 tmp = MIN_int32_T;
4630 }
4631 } else {
4632 tmp = MAX_int32_T;
4633 }
4634
4635 motor_Y.JD_Out = tmp;
4636 } else if (motor_DWork.temporalCounter_i1 >= (uint32_T)(7.0 /
4637 motor_DWork.Ts)) {
4638 /* Transition: '<S1>:1040' */
4639 motor_Y.Flag_Down_GZ_limit = 0U;
4640
4641 /* 下限位开关故障$/ */
4642 motor_DWork.is_HY_h = motor_IN_Int2_i;
4643 motor_DWork.temporalCounter_i1 = 0U;
4644
4645 /* Entry 'Int2': '<S1>:283' */
4646 motor_DWork.chu_jd -= 0.002;
4647
4648 /* Inport: '<Root>/JD_In' */
4649 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
4650 motor_B.JD_In_f = motor_U.JD_In;
4651 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
4652
4653 /* Inport: '<Root>/System_Order' */
4654 motor_B.Slect_port_c = motor_U.System_Order;
4655
4656 /* Inport: '<Root>/Encode_Sp' */
4657 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
4658
4659 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
4660 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
4661 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
4662 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
4663 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
4664 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
4665 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
4666 &motor_DWork.chu_jd);
4667
4668 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
4669 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
4670 tmp_0 = motor_B.Motor_HYDG.KP_e;
4671 if (tmp_0 < 2.147483648E+9) {
4672 if (tmp_0 >= -2.147483648E+9) {
4673 tmp = (int32_T)tmp_0;
4674 } else {
4675 tmp = MIN_int32_T;
4676 }
4677 } else {
4678 tmp = MAX_int32_T;
4679 }
4680
4681 motor_Y.JD_Error = tmp;
4682 tmp_0 = motor_B.Motor_HYDG.KP_JD;
4683 if (tmp_0 < 2.147483648E+9) {
4684 if (tmp_0 >= -2.147483648E+9) {
4685 tmp = (int32_T)tmp_0;
4686 } else {
4687 tmp = MIN_int32_T;
4688 }
4689 } else {
4690 tmp = MAX_int32_T;
4691 }
4692
4693 motor_Y.JD_Out = tmp;
4694 } else {
4695 motor_DWork.chu_jd -= 0.002;
4696
4697 /* Inport: '<Root>/JD_In' */
4698 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
4699 motor_B.JD_In_f = motor_U.JD_In;
4700 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
4701
4702 /* Inport: '<Root>/System_Order' */
4703 motor_B.Slect_port_c = motor_U.System_Order;
4704
4705 /* Inport: '<Root>/Encode_Sp' */
4706 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
4707
4708 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
4709 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
4710 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
4711 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
4712 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
4713 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
4714 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
4715 &motor_DWork.chu_jd);
4716
4717 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
4718 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
4719 tmp_0 = motor_B.Motor_HYDG.KP_e;
4720 if (tmp_0 < 2.147483648E+9) {
4721 if (tmp_0 >= -2.147483648E+9) {
4722 tmp = (int32_T)tmp_0;
4723 } else {
4724 tmp = MIN_int32_T;
4725 }
4726 } else {
4727 tmp = MAX_int32_T;
4728 }
4729
4730 motor_Y.JD_Error = tmp;
4731 tmp_0 = motor_B.Motor_HYDG.KP_JD;
4732 if (tmp_0 < 2.147483648E+9) {
4733 if (tmp_0 >= -2.147483648E+9) {
4734 tmp = (int32_T)tmp_0;
4735 } else {
4736 tmp = MIN_int32_T;
4737 }
4738 } else {
4739 tmp = MAX_int32_T;
4740 }
4741
4742 motor_Y.JD_Out = tmp;
4743 }
4744 break;
4745
4746 case motor_IN_Int2_i:
4747 /* During 'Int2': '<S1>:283' */
4748 if (motor_DWork.temporalCounter_i1 >= 50U) {
4749 /* Transition: '<S1>:713' */
4750 motor_DWork.is_HY_h = motor_IN_Int7;
4751 motor_DWork.temporalCounter_i1 = 0U;
4752 } else {
4753 motor_DWork.chu_jd -= 0.002;
4754
4755 /* Inport: '<Root>/JD_In' */
4756 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
4757 motor_B.JD_In_f = motor_U.JD_In;
4758 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
4759
4760 /* Inport: '<Root>/System_Order' */
4761 motor_B.Slect_port_c = motor_U.System_Order;
4762
4763 /* Inport: '<Root>/Encode_Sp' */
4764 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
4765
4766 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
4767 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
4768 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
4769 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
4770 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
4771 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
4772 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
4773 &motor_DWork.chu_jd);
4774
4775 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
4776 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
4777 tmp_0 = motor_B.Motor_HYDG.KP_e;
4778 if (tmp_0 < 2.147483648E+9) {
4779 if (tmp_0 >= -2.147483648E+9) {
4780 tmp = (int32_T)tmp_0;
4781 } else {
4782 tmp = MIN_int32_T;
4783 }
4784 } else {
4785 tmp = MAX_int32_T;
4786 }
4787
4788 motor_Y.JD_Error = tmp;
4789 tmp_0 = motor_B.Motor_HYDG.KP_JD;
4790 if (tmp_0 < 2.147483648E+9) {
4791 if (tmp_0 >= -2.147483648E+9) {
4792 tmp = (int32_T)tmp_0;
4793 } else {
4794 tmp = MIN_int32_T;
4795 }
4796 } else {
4797 tmp = MAX_int32_T;
4798 }
4799
4800 motor_Y.JD_Out = tmp;
4801 }
4802 break;
4803
4804 case motor_IN_Int3_l:
4805 /* During 'Int3': '<S1>:284' */
4806 if (fabs(motor_DWork.chu_jd) < 0.1) {
4807 /* Transition: '<S1>:714' */
4808 motor_DWork.is_HY_h = motor_IN_Int4_b;
4809 motor_DWork.temporalCounter_i1 = 0U;
4810
4811 /* Entry 'Int4': '<S1>:286' */
4812 motor_DWork.chu_jd = 0.0;
4813
4814 /* Inport: '<Root>/JD_In' */
4815 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
4816 motor_B.JD_In_f = motor_U.JD_In;
4817 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
4818
4819 /* Inport: '<Root>/System_Order' */
4820 motor_B.Slect_port_c = motor_U.System_Order;
4821
4822 /* Inport: '<Root>/Encode_Sp' */
4823 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
4824
4825 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
4826 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
4827 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
4828 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
4829 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
4830 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
4831 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
4832 &motor_DWork.chu_jd);
4833
4834 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
4835 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
4836 tmp_0 = motor_B.Motor_HYDG.KP_e;
4837 if (tmp_0 < 2.147483648E+9) {
4838 if (tmp_0 >= -2.147483648E+9) {
4839 tmp = (int32_T)tmp_0;
4840 } else {
4841 tmp = MIN_int32_T;
4842 }
4843 } else {
4844 tmp = MAX_int32_T;
4845 }
4846
4847 motor_Y.JD_Error = tmp;
4848 tmp_0 = motor_B.Motor_HYDG.KP_JD;
4849 if (tmp_0 < 2.147483648E+9) {
4850 if (tmp_0 >= -2.147483648E+9) {
4851 tmp = (int32_T)tmp_0;
4852 } else {
4853 tmp = MIN_int32_T;
4854 }
4855 } else {
4856 tmp = MAX_int32_T;
4857 }
4858
4859 motor_Y.JD_Out = tmp;
4860 } else {
4861 motor_DWork.chu_jd += 0.01;
4862
4863 /* Inport: '<Root>/JD_In' */
4864 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
4865 motor_B.JD_In_f = motor_U.JD_In;
4866 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
4867
4868 /* Inport: '<Root>/System_Order' */
4869 motor_B.Slect_port_c = motor_U.System_Order;
4870
4871 /* Inport: '<Root>/Encode_Sp' */
4872 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
4873
4874 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
4875 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
4876 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
4877 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
4878 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
4879 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
4880 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
4881 &motor_DWork.chu_jd);
4882
4883 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
4884 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
4885 tmp_0 = motor_B.Motor_HYDG.KP_e;
4886 if (tmp_0 < 2.147483648E+9) {
4887 if (tmp_0 >= -2.147483648E+9) {
4888 tmp = (int32_T)tmp_0;
4889 } else {
4890 tmp = MIN_int32_T;
4891 }
4892 } else {
4893 tmp = MAX_int32_T;
4894 }
4895
4896 motor_Y.JD_Error = tmp;
4897 tmp_0 = motor_B.Motor_HYDG.KP_JD;
4898 if (tmp_0 < 2.147483648E+9) {
4899 if (tmp_0 >= -2.147483648E+9) {
4900 tmp = (int32_T)tmp_0;
4901 } else {
4902 tmp = MIN_int32_T;
4903 }
4904 } else {
4905 tmp = MAX_int32_T;
4906 }
4907
4908 motor_Y.JD_Out = tmp;
4909 }
4910 break;
4911
4912 case motor_IN_Int4_b:
4913 /* During 'Int4': '<S1>:286' */
4914 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
4915 /* Transition: '<S1>:716' */
4916 motor_DWork.is_HY_h = motor_IN_Int5_a;
4917 motor_DWork.temporalCounter_i1 = 0U;
4918
4919 /* Entry 'Int5': '<S1>:287' */
4920 motor_Y.Motor_En = true;
4921
4922 /* 电机失能 */
4923 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
4924 } else {
4925 /* Inport: '<Root>/JD_In' */
4926 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
4927 motor_B.JD_In_f = motor_U.JD_In;
4928 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
4929
4930 /* Inport: '<Root>/System_Order' */
4931 motor_B.Slect_port_c = motor_U.System_Order;
4932
4933 /* Inport: '<Root>/Encode_Sp' */
4934 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
4935
4936 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
4937 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
4938 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
4939 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
4940 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
4941 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
4942 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
4943 &motor_DWork.chu_jd);
4944
4945 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
4946 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
4947 tmp_0 = motor_B.Motor_HYDG.KP_e;
4948 if (tmp_0 < 2.147483648E+9) {
4949 if (tmp_0 >= -2.147483648E+9) {
4950 tmp = (int32_T)tmp_0;
4951 } else {
4952 tmp = MIN_int32_T;
4953 }
4954 } else {
4955 tmp = MAX_int32_T;
4956 }
4957
4958 motor_Y.JD_Error = tmp;
4959 tmp_0 = motor_B.Motor_HYDG.KP_JD;
4960 if (tmp_0 < 2.147483648E+9) {
4961 if (tmp_0 >= -2.147483648E+9) {
4962 tmp = (int32_T)tmp_0;
4963 } else {
4964 tmp = MIN_int32_T;
4965 }
4966 } else {
4967 tmp = MAX_int32_T;
4968 }
4969
4970 motor_Y.JD_Out = tmp;
4971 }
4972 break;
4973
4974 case motor_IN_Int5_a:
4975 /* During 'Int5': '<S1>:287' */
4976 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
4977 /* Transition: '<S1>:715' */
4978 motor_DWork.is_HY_h = motor_IN_Int6_m;
4979
4980 /* Entry 'Int6': '<S1>:285' */
4981 motor_Y.DCZD = true;
4982
4983 /* 开启制动 */
4984 }
4985 break;
4986
4987 case motor_IN_Int6_m:
4988 /* During 'Int6': '<S1>:285' */
4989 /* Transition: '<S1>:712' */
4990 /* Transition: '<S1>:674' */
4991 motor_DWork.is_HY_h = motor_IN_NO_ACTIVE_CHILD;
4992 motor_DWork.is_Limit_Down_Test = motor_IN_NO_ACTIVE_CHILD;
4993 motor_DWork.is_Test_Mode = motor_IN_defult;
4994
4995 /* Entry 'defult': '<S1>:239' */
4996 motor_Y.Open_Result = 1U;
4997
4998 /* 开机状态位成功 */
4999 motor_DWork.In_State = 2U;
5000 break;
5001
5002 case motor_IN_Int7:
5003 /* During 'Int7': '<S1>:975' */
5004 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
5005 /* Transition: '<S1>:976' */
5006 motor_DWork.is_HY_h = motor_IN_Int3_l;
5007
5008 /* Entry 'Int3': '<S1>:284' */
5009 motor_DWork.chu_jd += 0.01;
5010
5011 /* Inport: '<Root>/JD_In' */
5012 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
5013 motor_B.JD_In_f = motor_U.JD_In;
5014 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
5015
5016 /* Inport: '<Root>/System_Order' */
5017 motor_B.Slect_port_c = motor_U.System_Order;
5018
5019 /* Inport: '<Root>/Encode_Sp' */
5020 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
5021
5022 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
5023 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
5024 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
5025 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
5026 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
5027 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
5028 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5029 &motor_DWork.chu_jd);
5030
5031 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
5032 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
5033 tmp_0 = motor_B.Motor_HYDG.KP_e;
5034 if (tmp_0 < 2.147483648E+9) {
5035 if (tmp_0 >= -2.147483648E+9) {
5036 tmp = (int32_T)tmp_0;
5037 } else {
5038 tmp = MIN_int32_T;
5039 }
5040 } else {
5041 tmp = MAX_int32_T;
5042 }
5043
5044 motor_Y.JD_Error = tmp;
5045 tmp_0 = motor_B.Motor_HYDG.KP_JD;
5046 if (tmp_0 < 2.147483648E+9) {
5047 if (tmp_0 >= -2.147483648E+9) {
5048 tmp = (int32_T)tmp_0;
5049 } else {
5050 tmp = MIN_int32_T;
5051 }
5052 } else {
5053 tmp = MAX_int32_T;
5054 }
5055
5056 motor_Y.JD_Out = tmp;
5057 } else {
5058 /* Inport: '<Root>/JD_In' */
5059 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
5060 motor_B.JD_In_f = motor_U.JD_In;
5061 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
5062
5063 /* Inport: '<Root>/System_Order' */
5064 motor_B.Slect_port_c = motor_U.System_Order;
5065
5066 /* Inport: '<Root>/Encode_Sp' */
5067 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
5068
5069 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
5070 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
5071 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
5072 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
5073 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
5074 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
5075 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5076 &motor_DWork.chu_jd);
5077
5078 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
5079 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
5080 tmp_0 = motor_B.Motor_HYDG.KP_e;
5081 if (tmp_0 < 2.147483648E+9) {
5082 if (tmp_0 >= -2.147483648E+9) {
5083 tmp = (int32_T)tmp_0;
5084 } else {
5085 tmp = MIN_int32_T;
5086 }
5087 } else {
5088 tmp = MAX_int32_T;
5089 }
5090
5091 motor_Y.JD_Error = tmp;
5092 tmp_0 = motor_B.Motor_HYDG.KP_JD;
5093 if (tmp_0 < 2.147483648E+9) {
5094 if (tmp_0 >= -2.147483648E+9) {
5095 tmp = (int32_T)tmp_0;
5096 } else {
5097 tmp = MIN_int32_T;
5098 }
5099 } else {
5100 tmp = MAX_int32_T;
5101 }
5102
5103 motor_Y.JD_Out = tmp;
5104 }
5105 break;
5106
5107 default:
5108 /* During 'Int8': '<S1>:995' */
5109 if (motor_DWork.chu_jd < -14.0) {
5110 /* Transition: '<S1>:997' */
5111 motor_DWork.is_HY_h = motor_IN_Int1;
5112 motor_DWork.temporalCounter_i1 = 0U;
5113
5114 /* Entry 'Int1': '<S1>:282' */
5115 motor_DWork.chu_jd -= 0.002;
5116
5117 /* Inport: '<Root>/JD_In' */
5118 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
5119 motor_B.JD_In_f = motor_U.JD_In;
5120 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
5121
5122 /* Inport: '<Root>/System_Order' */
5123 motor_B.Slect_port_c = motor_U.System_Order;
5124
5125 /* Inport: '<Root>/Encode_Sp' */
5126 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
5127
5128 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
5129 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
5130 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
5131 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
5132 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
5133 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
5134 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5135 &motor_DWork.chu_jd);
5136
5137 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
5138 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
5139 tmp_0 = motor_B.Motor_HYDG.KP_e;
5140 if (tmp_0 < 2.147483648E+9) {
5141 if (tmp_0 >= -2.147483648E+9) {
5142 tmp = (int32_T)tmp_0;
5143 } else {
5144 tmp = MIN_int32_T;
5145 }
5146 } else {
5147 tmp = MAX_int32_T;
5148 }
5149
5150 motor_Y.JD_Error = tmp;
5151 tmp_0 = motor_B.Motor_HYDG.KP_JD;
5152 if (tmp_0 < 2.147483648E+9) {
5153 if (tmp_0 >= -2.147483648E+9) {
5154 tmp = (int32_T)tmp_0;
5155 } else {
5156 tmp = MIN_int32_T;
5157 }
5158 } else {
5159 tmp = MAX_int32_T;
5160 }
5161
5162 motor_Y.JD_Out = tmp;
5163 } else if (motor_U.Down_Limit == 0) {
5164 /* Transition: '<S1>:996' */
5165 /* 下限位开关低电平有效 */
5166 motor_DWork.is_HY_h = motor_IN_Int2_i;
5167 motor_DWork.temporalCounter_i1 = 0U;
5168
5169 /* Entry 'Int2': '<S1>:283' */
5170 motor_DWork.chu_jd -= 0.002;
5171
5172 /* Inport: '<Root>/JD_In' */
5173 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
5174 motor_B.JD_In_f = motor_U.JD_In;
5175 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
5176
5177 /* Inport: '<Root>/System_Order' */
5178 motor_B.Slect_port_c = motor_U.System_Order;
5179
5180 /* Inport: '<Root>/Encode_Sp' */
5181 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
5182
5183 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
5184 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
5185 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
5186 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
5187 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
5188 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
5189 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5190 &motor_DWork.chu_jd);
5191
5192 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
5193 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
5194 tmp_0 = motor_B.Motor_HYDG.KP_e;
5195 if (tmp_0 < 2.147483648E+9) {
5196 if (tmp_0 >= -2.147483648E+9) {
5197 tmp = (int32_T)tmp_0;
5198 } else {
5199 tmp = MIN_int32_T;
5200 }
5201 } else {
5202 tmp = MAX_int32_T;
5203 }
5204
5205 motor_Y.JD_Error = tmp;
5206 tmp_0 = motor_B.Motor_HYDG.KP_JD;
5207 if (tmp_0 < 2.147483648E+9) {
5208 if (tmp_0 >= -2.147483648E+9) {
5209 tmp = (int32_T)tmp_0;
5210 } else {
5211 tmp = MIN_int32_T;
5212 }
5213 } else {
5214 tmp = MAX_int32_T;
5215 }
5216
5217 motor_Y.JD_Out = tmp;
5218 } else {
5219 motor_DWork.chu_jd -= 0.01;
5220
5221 /* Inport: '<Root>/JD_In' */
5222 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
5223 motor_B.JD_In_f = motor_U.JD_In;
5224 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
5225
5226 /* Inport: '<Root>/System_Order' */
5227 motor_B.Slect_port_c = motor_U.System_Order;
5228
5229 /* Inport: '<Root>/Encode_Sp' */
5230 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
5231
5232 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
5233 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
5234 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
5235 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
5236 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
5237 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
5238 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5239 &motor_DWork.chu_jd);
5240
5241 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
5242 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
5243 tmp_0 = motor_B.Motor_HYDG.KP_e;
5244 if (tmp_0 < 2.147483648E+9) {
5245 if (tmp_0 >= -2.147483648E+9) {
5246 tmp = (int32_T)tmp_0;
5247 } else {
5248 tmp = MIN_int32_T;
5249 }
5250 } else {
5251 tmp = MAX_int32_T;
5252 }
5253
5254 motor_Y.JD_Error = tmp;
5255 tmp_0 = motor_B.Motor_HYDG.KP_JD;
5256 if (tmp_0 < 2.147483648E+9) {
5257 if (tmp_0 >= -2.147483648E+9) {
5258 tmp = (int32_T)tmp_0;
5259 } else {
5260 tmp = MIN_int32_T;
5261 }
5262 } else {
5263 tmp = MAX_int32_T;
5264 }
5265
5266 motor_Y.JD_Out = tmp;
5267 }
5268 break;
5269 }
5270 break;
5271
5272 case motor_IN_XHHY_m1:
5273 /* During 'XHHY': '<S1>:296' */
5274 switch (motor_DWork.is_XHHY_f) {
5275 case motor_IN_Int:
5276 /* During 'Int': '<S1>:297' */
5277 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
5278 /* Transition: '<S1>:726' */
5279 motor_DWork.is_XHHY_f = motor_IN_Int8;
5280 }
5281 break;
5282
5283 case motor_IN_Int1:
5284 /* Inport: '<Root>/Down_Limit' */
5285 /* During 'Int1': '<S1>:298' */
5286 if (motor_U.Down_Limit == 0) {
5287 /* Transition: '<S1>:727' */
5288 /* 下限位开关低电平有效 */
5289 motor_DWork.is_XHHY_f = motor_IN_Int2_i;
5290 motor_DWork.temporalCounter_i1 = 0U;
5291
5292 /* Entry 'Int2': '<S1>:299' */
5293 motor_DWork.chu_jd -= 0.002;
5294
5295 /* Inport: '<Root>/JD_In' */
5296 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
5297 motor_B.JD_In = motor_U.JD_In;
5298 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
5299
5300 /* Inport: '<Root>/Encode_Sp' */
5301 motor_B.Encode_Sp = motor_U.Encode_Sp;
5302
5303 /* Inport: '<Root>/System_Order' */
5304 motor_B.Slect_port = motor_U.System_Order;
5305
5306 /* Inport: '<Root>/SGWY_In' */
5307 motor_B.SGWY_In = motor_U.SGWY_In;
5308
5309 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
5310 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
5311 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
5312 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
5313 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
5314 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
5315 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
5316 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
5317 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5318 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
5319 &motor_DWork.chu_jd);
5320
5321 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
5322 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
5323 if (tmp_0 < 65536.0) {
5324 if (tmp_0 >= 0.0) {
5325 tmp_1 = (uint16_T)tmp_0;
5326 } else {
5327 tmp_1 = 0U;
5328 }
5329 } else {
5330 tmp_1 = MAX_uint16_T;
5331 }
5332
5333 motor_Y.PWMOUT = tmp_1;
5334 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
5335 if (tmp_0 < 2.147483648E+9) {
5336 if (tmp_0 >= -2.147483648E+9) {
5337 tmp = (int32_T)tmp_0;
5338 } else {
5339 tmp = MIN_int32_T;
5340 }
5341 } else {
5342 tmp = MAX_int32_T;
5343 }
5344
5345 motor_Y.JD_Out = tmp;
5346 tmp_0 = motor_B.Motor_XHHY.KP_e;
5347 if (tmp_0 < 2.147483648E+9) {
5348 if (tmp_0 >= -2.147483648E+9) {
5349 tmp = (int32_T)tmp_0;
5350 } else {
5351 tmp = MIN_int32_T;
5352 }
5353 } else {
5354 tmp = MAX_int32_T;
5355 }
5356
5357 motor_Y.JD_Error = tmp;
5358 } else if (motor_DWork.temporalCounter_i1 >= (uint32_T)(5.0 /
5359 motor_DWork.Ts)) {
5360 /* Transition: '<S1>:1041' */
5361 motor_Y.Flag_Down_GZ_limit = 0U;
5362
5363 /* 下限位开关故障$/ */
5364 motor_DWork.is_XHHY_f = motor_IN_Int2_i;
5365 motor_DWork.temporalCounter_i1 = 0U;
5366
5367 /* Entry 'Int2': '<S1>:299' */
5368 motor_DWork.chu_jd -= 0.002;
5369
5370 /* Inport: '<Root>/JD_In' */
5371 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
5372 motor_B.JD_In = motor_U.JD_In;
5373 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
5374
5375 /* Inport: '<Root>/Encode_Sp' */
5376 motor_B.Encode_Sp = motor_U.Encode_Sp;
5377
5378 /* Inport: '<Root>/System_Order' */
5379 motor_B.Slect_port = motor_U.System_Order;
5380
5381 /* Inport: '<Root>/SGWY_In' */
5382 motor_B.SGWY_In = motor_U.SGWY_In;
5383
5384 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
5385 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
5386 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
5387 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
5388 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
5389 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
5390 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
5391 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
5392 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5393 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
5394 &motor_DWork.chu_jd);
5395
5396 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
5397 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
5398 if (tmp_0 < 65536.0) {
5399 if (tmp_0 >= 0.0) {
5400 tmp_1 = (uint16_T)tmp_0;
5401 } else {
5402 tmp_1 = 0U;
5403 }
5404 } else {
5405 tmp_1 = MAX_uint16_T;
5406 }
5407
5408 motor_Y.PWMOUT = tmp_1;
5409 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
5410 if (tmp_0 < 2.147483648E+9) {
5411 if (tmp_0 >= -2.147483648E+9) {
5412 tmp = (int32_T)tmp_0;
5413 } else {
5414 tmp = MIN_int32_T;
5415 }
5416 } else {
5417 tmp = MAX_int32_T;
5418 }
5419
5420 motor_Y.JD_Out = tmp;
5421 tmp_0 = motor_B.Motor_XHHY.KP_e;
5422 if (tmp_0 < 2.147483648E+9) {
5423 if (tmp_0 >= -2.147483648E+9) {
5424 tmp = (int32_T)tmp_0;
5425 } else {
5426 tmp = MIN_int32_T;
5427 }
5428 } else {
5429 tmp = MAX_int32_T;
5430 }
5431
5432 motor_Y.JD_Error = tmp;
5433 } else {
5434 motor_DWork.chu_jd -= 0.002;
5435
5436 /* Inport: '<Root>/JD_In' */
5437 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
5438 motor_B.JD_In = motor_U.JD_In;
5439 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
5440
5441 /* Inport: '<Root>/Encode_Sp' */
5442 motor_B.Encode_Sp = motor_U.Encode_Sp;
5443
5444 /* Inport: '<Root>/System_Order' */
5445 motor_B.Slect_port = motor_U.System_Order;
5446
5447 /* Inport: '<Root>/SGWY_In' */
5448 motor_B.SGWY_In = motor_U.SGWY_In;
5449
5450 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
5451 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
5452 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
5453 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
5454 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
5455 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
5456 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
5457 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
5458 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5459 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
5460 &motor_DWork.chu_jd);
5461
5462 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
5463 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
5464 if (tmp_0 < 65536.0) {
5465 if (tmp_0 >= 0.0) {
5466 tmp_1 = (uint16_T)tmp_0;
5467 } else {
5468 tmp_1 = 0U;
5469 }
5470 } else {
5471 tmp_1 = MAX_uint16_T;
5472 }
5473
5474 motor_Y.PWMOUT = tmp_1;
5475 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
5476 if (tmp_0 < 2.147483648E+9) {
5477 if (tmp_0 >= -2.147483648E+9) {
5478 tmp = (int32_T)tmp_0;
5479 } else {
5480 tmp = MIN_int32_T;
5481 }
5482 } else {
5483 tmp = MAX_int32_T;
5484 }
5485
5486 motor_Y.JD_Out = tmp;
5487 tmp_0 = motor_B.Motor_XHHY.KP_e;
5488 if (tmp_0 < 2.147483648E+9) {
5489 if (tmp_0 >= -2.147483648E+9) {
5490 tmp = (int32_T)tmp_0;
5491 } else {
5492 tmp = MIN_int32_T;
5493 }
5494 } else {
5495 tmp = MAX_int32_T;
5496 }
5497
5498 motor_Y.JD_Error = tmp;
5499 }
5500 break;
5501
5502 case motor_IN_Int2_i:
5503 /* During 'Int2': '<S1>:299' */
5504 if (motor_DWork.temporalCounter_i1 >= 50U) {
5505 /* Transition: '<S1>:729' */
5506 motor_DWork.is_XHHY_f = motor_IN_Int7;
5507 motor_DWork.temporalCounter_i1 = 0U;
5508 } else {
5509 motor_DWork.chu_jd -= 0.002;
5510
5511 /* Inport: '<Root>/JD_In' */
5512 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
5513 motor_B.JD_In = motor_U.JD_In;
5514 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
5515
5516 /* Inport: '<Root>/Encode_Sp' */
5517 motor_B.Encode_Sp = motor_U.Encode_Sp;
5518
5519 /* Inport: '<Root>/System_Order' */
5520 motor_B.Slect_port = motor_U.System_Order;
5521
5522 /* Inport: '<Root>/SGWY_In' */
5523 motor_B.SGWY_In = motor_U.SGWY_In;
5524
5525 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
5526 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
5527 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
5528 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
5529 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
5530 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
5531 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
5532 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
5533 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5534 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
5535 &motor_DWork.chu_jd);
5536
5537 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
5538 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
5539 if (tmp_0 < 65536.0) {
5540 if (tmp_0 >= 0.0) {
5541 tmp_1 = (uint16_T)tmp_0;
5542 } else {
5543 tmp_1 = 0U;
5544 }
5545 } else {
5546 tmp_1 = MAX_uint16_T;
5547 }
5548
5549 motor_Y.PWMOUT = tmp_1;
5550 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
5551 if (tmp_0 < 2.147483648E+9) {
5552 if (tmp_0 >= -2.147483648E+9) {
5553 tmp = (int32_T)tmp_0;
5554 } else {
5555 tmp = MIN_int32_T;
5556 }
5557 } else {
5558 tmp = MAX_int32_T;
5559 }
5560
5561 motor_Y.JD_Out = tmp;
5562 tmp_0 = motor_B.Motor_XHHY.KP_e;
5563 if (tmp_0 < 2.147483648E+9) {
5564 if (tmp_0 >= -2.147483648E+9) {
5565 tmp = (int32_T)tmp_0;
5566 } else {
5567 tmp = MIN_int32_T;
5568 }
5569 } else {
5570 tmp = MAX_int32_T;
5571 }
5572
5573 motor_Y.JD_Error = tmp;
5574 }
5575 break;
5576
5577 case motor_IN_Int3_l:
5578 /* During 'Int3': '<S1>:300' */
5579 if (fabs(motor_DWork.chu_jd) < 0.1) {
5580 /* Transition: '<S1>:730' */
5581 motor_DWork.is_XHHY_f = motor_IN_Int4_b;
5582 motor_DWork.temporalCounter_i1 = 0U;
5583
5584 /* Entry 'Int4': '<S1>:302' */
5585 motor_DWork.chu_jd = 0.0;
5586
5587 /* Inport: '<Root>/JD_In' */
5588 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
5589 motor_B.JD_In = motor_U.JD_In;
5590 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
5591
5592 /* Inport: '<Root>/Encode_Sp' */
5593 motor_B.Encode_Sp = motor_U.Encode_Sp;
5594
5595 /* Inport: '<Root>/System_Order' */
5596 motor_B.Slect_port = motor_U.System_Order;
5597
5598 /* Inport: '<Root>/SGWY_In' */
5599 motor_B.SGWY_In = motor_U.SGWY_In;
5600
5601 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
5602 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
5603 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
5604 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
5605 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
5606 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
5607 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
5608 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
5609 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5610 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
5611 &motor_DWork.chu_jd);
5612
5613 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
5614 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
5615 if (tmp_0 < 65536.0) {
5616 if (tmp_0 >= 0.0) {
5617 tmp_1 = (uint16_T)tmp_0;
5618 } else {
5619 tmp_1 = 0U;
5620 }
5621 } else {
5622 tmp_1 = MAX_uint16_T;
5623 }
5624
5625 motor_Y.PWMOUT = tmp_1;
5626 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
5627 if (tmp_0 < 2.147483648E+9) {
5628 if (tmp_0 >= -2.147483648E+9) {
5629 tmp = (int32_T)tmp_0;
5630 } else {
5631 tmp = MIN_int32_T;
5632 }
5633 } else {
5634 tmp = MAX_int32_T;
5635 }
5636
5637 motor_Y.JD_Out = tmp;
5638 tmp_0 = motor_B.Motor_XHHY.KP_e;
5639 if (tmp_0 < 2.147483648E+9) {
5640 if (tmp_0 >= -2.147483648E+9) {
5641 tmp = (int32_T)tmp_0;
5642 } else {
5643 tmp = MIN_int32_T;
5644 }
5645 } else {
5646 tmp = MAX_int32_T;
5647 }
5648
5649 motor_Y.JD_Error = tmp;
5650 } else {
5651 motor_DWork.chu_jd += 0.01;
5652
5653 /* Inport: '<Root>/JD_In' */
5654 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
5655 motor_B.JD_In = motor_U.JD_In;
5656 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
5657
5658 /* Inport: '<Root>/Encode_Sp' */
5659 motor_B.Encode_Sp = motor_U.Encode_Sp;
5660
5661 /* Inport: '<Root>/System_Order' */
5662 motor_B.Slect_port = motor_U.System_Order;
5663
5664 /* Inport: '<Root>/SGWY_In' */
5665 motor_B.SGWY_In = motor_U.SGWY_In;
5666
5667 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
5668 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
5669 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
5670 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
5671 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
5672 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
5673 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
5674 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
5675 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5676 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
5677 &motor_DWork.chu_jd);
5678
5679 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
5680 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
5681 if (tmp_0 < 65536.0) {
5682 if (tmp_0 >= 0.0) {
5683 tmp_1 = (uint16_T)tmp_0;
5684 } else {
5685 tmp_1 = 0U;
5686 }
5687 } else {
5688 tmp_1 = MAX_uint16_T;
5689 }
5690
5691 motor_Y.PWMOUT = tmp_1;
5692 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
5693 if (tmp_0 < 2.147483648E+9) {
5694 if (tmp_0 >= -2.147483648E+9) {
5695 tmp = (int32_T)tmp_0;
5696 } else {
5697 tmp = MIN_int32_T;
5698 }
5699 } else {
5700 tmp = MAX_int32_T;
5701 }
5702
5703 motor_Y.JD_Out = tmp;
5704 tmp_0 = motor_B.Motor_XHHY.KP_e;
5705 if (tmp_0 < 2.147483648E+9) {
5706 if (tmp_0 >= -2.147483648E+9) {
5707 tmp = (int32_T)tmp_0;
5708 } else {
5709 tmp = MIN_int32_T;
5710 }
5711 } else {
5712 tmp = MAX_int32_T;
5713 }
5714
5715 motor_Y.JD_Error = tmp;
5716 }
5717 break;
5718
5719 case motor_IN_Int4_b:
5720 /* During 'Int4': '<S1>:302' */
5721 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
5722 /* Transition: '<S1>:732' */
5723 motor_DWork.is_XHHY_f = motor_IN_Int5_a;
5724 motor_DWork.temporalCounter_i1 = 0U;
5725
5726 /* Entry 'Int5': '<S1>:303' */
5727 motor_Y.Motor_En = true;
5728
5729 /* 电机失能 */
5730 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
5731 } else {
5732 /* Inport: '<Root>/JD_In' */
5733 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
5734 motor_B.JD_In = motor_U.JD_In;
5735 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
5736
5737 /* Inport: '<Root>/Encode_Sp' */
5738 motor_B.Encode_Sp = motor_U.Encode_Sp;
5739
5740 /* Inport: '<Root>/System_Order' */
5741 motor_B.Slect_port = motor_U.System_Order;
5742
5743 /* Inport: '<Root>/SGWY_In' */
5744 motor_B.SGWY_In = motor_U.SGWY_In;
5745
5746 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
5747 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
5748 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
5749 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
5750 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
5751 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
5752 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
5753 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
5754 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5755 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
5756 &motor_DWork.chu_jd);
5757
5758 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
5759 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
5760 if (tmp_0 < 65536.0) {
5761 if (tmp_0 >= 0.0) {
5762 tmp_1 = (uint16_T)tmp_0;
5763 } else {
5764 tmp_1 = 0U;
5765 }
5766 } else {
5767 tmp_1 = MAX_uint16_T;
5768 }
5769
5770 motor_Y.PWMOUT = tmp_1;
5771 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
5772 if (tmp_0 < 2.147483648E+9) {
5773 if (tmp_0 >= -2.147483648E+9) {
5774 tmp = (int32_T)tmp_0;
5775 } else {
5776 tmp = MIN_int32_T;
5777 }
5778 } else {
5779 tmp = MAX_int32_T;
5780 }
5781
5782 motor_Y.JD_Out = tmp;
5783 tmp_0 = motor_B.Motor_XHHY.KP_e;
5784 if (tmp_0 < 2.147483648E+9) {
5785 if (tmp_0 >= -2.147483648E+9) {
5786 tmp = (int32_T)tmp_0;
5787 } else {
5788 tmp = MIN_int32_T;
5789 }
5790 } else {
5791 tmp = MAX_int32_T;
5792 }
5793
5794 motor_Y.JD_Error = tmp;
5795 }
5796 break;
5797
5798 case motor_IN_Int5_a:
5799 /* During 'Int5': '<S1>:303' */
5800 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
5801 /* Transition: '<S1>:731' */
5802 motor_DWork.is_XHHY_f = motor_IN_Int6_m;
5803
5804 /* Entry 'Int6': '<S1>:301' */
5805 motor_Y.DCZD = true;
5806
5807 /* 开启制动 */
5808 }
5809 break;
5810
5811 case motor_IN_Int6_m:
5812 /* During 'Int6': '<S1>:301' */
5813 /* Transition: '<S1>:728' */
5814 /* Transition: '<S1>:674' */
5815 motor_DWork.is_XHHY_f = motor_IN_NO_ACTIVE_CHILD;
5816 motor_DWork.is_Limit_Down_Test = motor_IN_NO_ACTIVE_CHILD;
5817 motor_DWork.is_Test_Mode = motor_IN_defult;
5818
5819 /* Entry 'defult': '<S1>:239' */
5820 motor_Y.Open_Result = 1U;
5821
5822 /* 开机状态位成功 */
5823 motor_DWork.In_State = 2U;
5824 break;
5825
5826 case motor_IN_Int7:
5827 /* During 'Int7': '<S1>:977' */
5828 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
5829 /* Transition: '<S1>:979' */
5830 motor_DWork.is_XHHY_f = motor_IN_Int3_l;
5831
5832 /* Entry 'Int3': '<S1>:300' */
5833 motor_DWork.chu_jd += 0.01;
5834
5835 /* Inport: '<Root>/JD_In' */
5836 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
5837 motor_B.JD_In = motor_U.JD_In;
5838 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
5839
5840 /* Inport: '<Root>/Encode_Sp' */
5841 motor_B.Encode_Sp = motor_U.Encode_Sp;
5842
5843 /* Inport: '<Root>/System_Order' */
5844 motor_B.Slect_port = motor_U.System_Order;
5845
5846 /* Inport: '<Root>/SGWY_In' */
5847 motor_B.SGWY_In = motor_U.SGWY_In;
5848
5849 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
5850 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
5851 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
5852 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
5853 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
5854 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
5855 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
5856 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
5857 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5858 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
5859 &motor_DWork.chu_jd);
5860
5861 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
5862 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
5863 if (tmp_0 < 65536.0) {
5864 if (tmp_0 >= 0.0) {
5865 tmp_1 = (uint16_T)tmp_0;
5866 } else {
5867 tmp_1 = 0U;
5868 }
5869 } else {
5870 tmp_1 = MAX_uint16_T;
5871 }
5872
5873 motor_Y.PWMOUT = tmp_1;
5874 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
5875 if (tmp_0 < 2.147483648E+9) {
5876 if (tmp_0 >= -2.147483648E+9) {
5877 tmp = (int32_T)tmp_0;
5878 } else {
5879 tmp = MIN_int32_T;
5880 }
5881 } else {
5882 tmp = MAX_int32_T;
5883 }
5884
5885 motor_Y.JD_Out = tmp;
5886 tmp_0 = motor_B.Motor_XHHY.KP_e;
5887 if (tmp_0 < 2.147483648E+9) {
5888 if (tmp_0 >= -2.147483648E+9) {
5889 tmp = (int32_T)tmp_0;
5890 } else {
5891 tmp = MIN_int32_T;
5892 }
5893 } else {
5894 tmp = MAX_int32_T;
5895 }
5896
5897 motor_Y.JD_Error = tmp;
5898 } else {
5899 /* Inport: '<Root>/JD_In' */
5900 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
5901 motor_B.JD_In = motor_U.JD_In;
5902 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
5903
5904 /* Inport: '<Root>/Encode_Sp' */
5905 motor_B.Encode_Sp = motor_U.Encode_Sp;
5906
5907 /* Inport: '<Root>/System_Order' */
5908 motor_B.Slect_port = motor_U.System_Order;
5909
5910 /* Inport: '<Root>/SGWY_In' */
5911 motor_B.SGWY_In = motor_U.SGWY_In;
5912
5913 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
5914 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
5915 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
5916 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
5917 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
5918 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
5919 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
5920 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
5921 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5922 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
5923 &motor_DWork.chu_jd);
5924
5925 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
5926 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
5927 if (tmp_0 < 65536.0) {
5928 if (tmp_0 >= 0.0) {
5929 tmp_1 = (uint16_T)tmp_0;
5930 } else {
5931 tmp_1 = 0U;
5932 }
5933 } else {
5934 tmp_1 = MAX_uint16_T;
5935 }
5936
5937 motor_Y.PWMOUT = tmp_1;
5938 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
5939 if (tmp_0 < 2.147483648E+9) {
5940 if (tmp_0 >= -2.147483648E+9) {
5941 tmp = (int32_T)tmp_0;
5942 } else {
5943 tmp = MIN_int32_T;
5944 }
5945 } else {
5946 tmp = MAX_int32_T;
5947 }
5948
5949 motor_Y.JD_Out = tmp;
5950 tmp_0 = motor_B.Motor_XHHY.KP_e;
5951 if (tmp_0 < 2.147483648E+9) {
5952 if (tmp_0 >= -2.147483648E+9) {
5953 tmp = (int32_T)tmp_0;
5954 } else {
5955 tmp = MIN_int32_T;
5956 }
5957 } else {
5958 tmp = MAX_int32_T;
5959 }
5960
5961 motor_Y.JD_Error = tmp;
5962 }
5963 break;
5964
5965 default:
5966 /* During 'Int8': '<S1>:998' */
5967 if (motor_DWork.chu_jd < -16.0) {
5968 /* Transition: '<S1>:999' */
5969 motor_DWork.is_XHHY_f = motor_IN_Int1;
5970 motor_DWork.temporalCounter_i1 = 0U;
5971
5972 /* Entry 'Int1': '<S1>:298' */
5973 motor_DWork.chu_jd -= 0.002;
5974
5975 /* Inport: '<Root>/JD_In' */
5976 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
5977 motor_B.JD_In = motor_U.JD_In;
5978 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
5979
5980 /* Inport: '<Root>/Encode_Sp' */
5981 motor_B.Encode_Sp = motor_U.Encode_Sp;
5982
5983 /* Inport: '<Root>/System_Order' */
5984 motor_B.Slect_port = motor_U.System_Order;
5985
5986 /* Inport: '<Root>/SGWY_In' */
5987 motor_B.SGWY_In = motor_U.SGWY_In;
5988
5989 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
5990 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
5991 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
5992 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
5993 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
5994 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
5995 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
5996 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
5997 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
5998 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
5999 &motor_DWork.chu_jd);
6000
6001 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
6002 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
6003 if (tmp_0 < 65536.0) {
6004 if (tmp_0 >= 0.0) {
6005 tmp_1 = (uint16_T)tmp_0;
6006 } else {
6007 tmp_1 = 0U;
6008 }
6009 } else {
6010 tmp_1 = MAX_uint16_T;
6011 }
6012
6013 motor_Y.PWMOUT = tmp_1;
6014 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
6015 if (tmp_0 < 2.147483648E+9) {
6016 if (tmp_0 >= -2.147483648E+9) {
6017 tmp = (int32_T)tmp_0;
6018 } else {
6019 tmp = MIN_int32_T;
6020 }
6021 } else {
6022 tmp = MAX_int32_T;
6023 }
6024
6025 motor_Y.JD_Out = tmp;
6026 tmp_0 = motor_B.Motor_XHHY.KP_e;
6027 if (tmp_0 < 2.147483648E+9) {
6028 if (tmp_0 >= -2.147483648E+9) {
6029 tmp = (int32_T)tmp_0;
6030 } else {
6031 tmp = MIN_int32_T;
6032 }
6033 } else {
6034 tmp = MAX_int32_T;
6035 }
6036
6037 motor_Y.JD_Error = tmp;
6038 } else if (motor_U.Down_Limit == 0) {
6039 /* Transition: '<S1>:1001' */
6040 /* 下限位开关低电平有效 */
6041 motor_DWork.is_XHHY_f = motor_IN_Int2_i;
6042 motor_DWork.temporalCounter_i1 = 0U;
6043
6044 /* Entry 'Int2': '<S1>:299' */
6045 motor_DWork.chu_jd -= 0.002;
6046
6047 /* Inport: '<Root>/JD_In' */
6048 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
6049 motor_B.JD_In = motor_U.JD_In;
6050 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
6051
6052 /* Inport: '<Root>/Encode_Sp' */
6053 motor_B.Encode_Sp = motor_U.Encode_Sp;
6054
6055 /* Inport: '<Root>/System_Order' */
6056 motor_B.Slect_port = motor_U.System_Order;
6057
6058 /* Inport: '<Root>/SGWY_In' */
6059 motor_B.SGWY_In = motor_U.SGWY_In;
6060
6061 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
6062 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
6063 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
6064 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
6065 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
6066 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6067 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
6068 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
6069 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
6070 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6071 &motor_DWork.chu_jd);
6072
6073 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
6074 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
6075 if (tmp_0 < 65536.0) {
6076 if (tmp_0 >= 0.0) {
6077 tmp_1 = (uint16_T)tmp_0;
6078 } else {
6079 tmp_1 = 0U;
6080 }
6081 } else {
6082 tmp_1 = MAX_uint16_T;
6083 }
6084
6085 motor_Y.PWMOUT = tmp_1;
6086 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
6087 if (tmp_0 < 2.147483648E+9) {
6088 if (tmp_0 >= -2.147483648E+9) {
6089 tmp = (int32_T)tmp_0;
6090 } else {
6091 tmp = MIN_int32_T;
6092 }
6093 } else {
6094 tmp = MAX_int32_T;
6095 }
6096
6097 motor_Y.JD_Out = tmp;
6098 tmp_0 = motor_B.Motor_XHHY.KP_e;
6099 if (tmp_0 < 2.147483648E+9) {
6100 if (tmp_0 >= -2.147483648E+9) {
6101 tmp = (int32_T)tmp_0;
6102 } else {
6103 tmp = MIN_int32_T;
6104 }
6105 } else {
6106 tmp = MAX_int32_T;
6107 }
6108
6109 motor_Y.JD_Error = tmp;
6110 } else {
6111 motor_DWork.chu_jd -= 0.01;
6112
6113 /* Inport: '<Root>/JD_In' */
6114 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
6115 motor_B.JD_In = motor_U.JD_In;
6116 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
6117
6118 /* Inport: '<Root>/Encode_Sp' */
6119 motor_B.Encode_Sp = motor_U.Encode_Sp;
6120
6121 /* Inport: '<Root>/System_Order' */
6122 motor_B.Slect_port = motor_U.System_Order;
6123
6124 /* Inport: '<Root>/SGWY_In' */
6125 motor_B.SGWY_In = motor_U.SGWY_In;
6126
6127 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
6128 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
6129 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
6130 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
6131 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
6132 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6133 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
6134 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
6135 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
6136 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6137 &motor_DWork.chu_jd);
6138
6139 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
6140 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
6141 if (tmp_0 < 65536.0) {
6142 if (tmp_0 >= 0.0) {
6143 tmp_1 = (uint16_T)tmp_0;
6144 } else {
6145 tmp_1 = 0U;
6146 }
6147 } else {
6148 tmp_1 = MAX_uint16_T;
6149 }
6150
6151 motor_Y.PWMOUT = tmp_1;
6152 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
6153 if (tmp_0 < 2.147483648E+9) {
6154 if (tmp_0 >= -2.147483648E+9) {
6155 tmp = (int32_T)tmp_0;
6156 } else {
6157 tmp = MIN_int32_T;
6158 }
6159 } else {
6160 tmp = MAX_int32_T;
6161 }
6162
6163 motor_Y.JD_Out = tmp;
6164 tmp_0 = motor_B.Motor_XHHY.KP_e;
6165 if (tmp_0 < 2.147483648E+9) {
6166 if (tmp_0 >= -2.147483648E+9) {
6167 tmp = (int32_T)tmp_0;
6168 } else {
6169 tmp = MIN_int32_T;
6170 }
6171 } else {
6172 tmp = MAX_int32_T;
6173 }
6174
6175 motor_Y.JD_Error = tmp;
6176 }
6177 break;
6178 }
6179 break;
6180
6181 default:
6182 motor_XHZY_as();
6183 break;
6184 }
6185}
6186
6187/* Function for Chart: '<Root>/Chart' */
6188static void motor_XHZY_a(void)
6189{
6190 int32_T tmp;
6191 uint16_T tmp_0;
6192 real_T tmp_1;
6193
6194 /* During 'XHZY': '<S1>:257' */
6195 switch (motor_DWork.is_XHZY_a) {
6196 case motor_IN_Int:
6197 /* During 'Int': '<S1>:259' */
6198 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
6199 /* Transition: '<S1>:694' */
6200 motor_DWork.is_XHZY_a = motor_IN_Int8;
6201 }
6202 break;
6203
6204 case motor_IN_Int1:
6205 /* Inport: '<Root>/Up_Limit' */
6206 /* During 'Int1': '<S1>:260' */
6207 if (motor_U.Up_Limit == 0) {
6208 /* Transition: '<S1>:695' */
6209 /* 上限位开关低电平有效 */
6210 motor_DWork.is_XHZY_a = motor_IN_Int2_i;
6211 motor_DWork.temporalCounter_i1 = 0U;
6212
6213 /* Entry 'Int2': '<S1>:261' */
6214 motor_DWork.chu_jd += 0.002;
6215
6216 /* Inport: '<Root>/JD_In' */
6217 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
6218 motor_B.JD_In_d = motor_U.JD_In;
6219 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
6220
6221 /* Inport: '<Root>/YJ_In' */
6222 motor_B.YJ_In = motor_U.YJ_In;
6223
6224 /* Inport: '<Root>/Encode_Sp' */
6225 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
6226
6227 /* Inport: '<Root>/System_Order' */
6228 motor_B.Slect_port_h = motor_U.System_Order;
6229
6230 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
6231 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
6232 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
6233 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
6234 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
6235 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6236 &motor_DWork.EN_extern, &motor_DWork.Forword,
6237 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
6238 &motor_DWork.KG_En, &motor_DWork.KG_JD,
6239 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
6240 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6241 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
6242 &motor_DWork.chu_jd);
6243
6244 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
6245 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
6246 if (tmp_1 < 65536.0) {
6247 if (tmp_1 >= 0.0) {
6248 tmp_0 = (uint16_T)tmp_1;
6249 } else {
6250 tmp_0 = 0U;
6251 }
6252 } else {
6253 tmp_0 = MAX_uint16_T;
6254 }
6255
6256 motor_Y.PWMOUT = tmp_0;
6257 tmp_1 = motor_B.Motor_XHZY.KP_JD;
6258 if (tmp_1 < 2.147483648E+9) {
6259 if (tmp_1 >= -2.147483648E+9) {
6260 tmp = (int32_T)tmp_1;
6261 } else {
6262 tmp = MIN_int32_T;
6263 }
6264 } else {
6265 tmp = MAX_int32_T;
6266 }
6267
6268 motor_Y.JD_Out = tmp;
6269 tmp_1 = motor_B.Motor_XHZY.KP_e;
6270 if (tmp_1 < 2.147483648E+9) {
6271 if (tmp_1 >= -2.147483648E+9) {
6272 tmp = (int32_T)tmp_1;
6273 } else {
6274 tmp = MIN_int32_T;
6275 }
6276 } else {
6277 tmp = MAX_int32_T;
6278 }
6279
6280 motor_Y.JD_Error = tmp;
6281 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
6282 } else if (motor_DWork.temporalCounter_i1 >= (uint32_T)(5.0 / motor_DWork.Ts))
6283 {
6284 /* Transition: '<S1>:1036' */
6285 motor_Y.Flag_Up_GZ_limit = 0U;
6286
6287 /* 上限位开关故障$/ */
6288 motor_DWork.is_XHZY_a = motor_IN_Int2_i;
6289 motor_DWork.temporalCounter_i1 = 0U;
6290
6291 /* Entry 'Int2': '<S1>:261' */
6292 motor_DWork.chu_jd += 0.002;
6293
6294 /* Inport: '<Root>/JD_In' */
6295 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
6296 motor_B.JD_In_d = motor_U.JD_In;
6297 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
6298
6299 /* Inport: '<Root>/YJ_In' */
6300 motor_B.YJ_In = motor_U.YJ_In;
6301
6302 /* Inport: '<Root>/Encode_Sp' */
6303 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
6304
6305 /* Inport: '<Root>/System_Order' */
6306 motor_B.Slect_port_h = motor_U.System_Order;
6307
6308 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
6309 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
6310 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
6311 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
6312 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
6313 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6314 &motor_DWork.EN_extern, &motor_DWork.Forword,
6315 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
6316 &motor_DWork.KG_En, &motor_DWork.KG_JD,
6317 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
6318 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6319 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
6320 &motor_DWork.chu_jd);
6321
6322 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
6323 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
6324 if (tmp_1 < 65536.0) {
6325 if (tmp_1 >= 0.0) {
6326 tmp_0 = (uint16_T)tmp_1;
6327 } else {
6328 tmp_0 = 0U;
6329 }
6330 } else {
6331 tmp_0 = MAX_uint16_T;
6332 }
6333
6334 motor_Y.PWMOUT = tmp_0;
6335 tmp_1 = motor_B.Motor_XHZY.KP_JD;
6336 if (tmp_1 < 2.147483648E+9) {
6337 if (tmp_1 >= -2.147483648E+9) {
6338 tmp = (int32_T)tmp_1;
6339 } else {
6340 tmp = MIN_int32_T;
6341 }
6342 } else {
6343 tmp = MAX_int32_T;
6344 }
6345
6346 motor_Y.JD_Out = tmp;
6347 tmp_1 = motor_B.Motor_XHZY.KP_e;
6348 if (tmp_1 < 2.147483648E+9) {
6349 if (tmp_1 >= -2.147483648E+9) {
6350 tmp = (int32_T)tmp_1;
6351 } else {
6352 tmp = MIN_int32_T;
6353 }
6354 } else {
6355 tmp = MAX_int32_T;
6356 }
6357
6358 motor_Y.JD_Error = tmp;
6359 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
6360 } else {
6361 motor_DWork.chu_jd += 0.002;
6362
6363 /* Inport: '<Root>/JD_In' */
6364 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
6365 motor_B.JD_In_d = motor_U.JD_In;
6366 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
6367
6368 /* Inport: '<Root>/YJ_In' */
6369 motor_B.YJ_In = motor_U.YJ_In;
6370
6371 /* Inport: '<Root>/Encode_Sp' */
6372 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
6373
6374 /* Inport: '<Root>/System_Order' */
6375 motor_B.Slect_port_h = motor_U.System_Order;
6376
6377 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
6378 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
6379 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
6380 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
6381 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
6382 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6383 &motor_DWork.EN_extern, &motor_DWork.Forword,
6384 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
6385 &motor_DWork.KG_En, &motor_DWork.KG_JD,
6386 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
6387 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6388 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
6389 &motor_DWork.chu_jd);
6390
6391 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
6392 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
6393 if (tmp_1 < 65536.0) {
6394 if (tmp_1 >= 0.0) {
6395 tmp_0 = (uint16_T)tmp_1;
6396 } else {
6397 tmp_0 = 0U;
6398 }
6399 } else {
6400 tmp_0 = MAX_uint16_T;
6401 }
6402
6403 motor_Y.PWMOUT = tmp_0;
6404 tmp_1 = motor_B.Motor_XHZY.KP_JD;
6405 if (tmp_1 < 2.147483648E+9) {
6406 if (tmp_1 >= -2.147483648E+9) {
6407 tmp = (int32_T)tmp_1;
6408 } else {
6409 tmp = MIN_int32_T;
6410 }
6411 } else {
6412 tmp = MAX_int32_T;
6413 }
6414
6415 motor_Y.JD_Out = tmp;
6416 tmp_1 = motor_B.Motor_XHZY.KP_e;
6417 if (tmp_1 < 2.147483648E+9) {
6418 if (tmp_1 >= -2.147483648E+9) {
6419 tmp = (int32_T)tmp_1;
6420 } else {
6421 tmp = MIN_int32_T;
6422 }
6423 } else {
6424 tmp = MAX_int32_T;
6425 }
6426
6427 motor_Y.JD_Error = tmp;
6428 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
6429 }
6430
6431 /* End of Inport: '<Root>/Up_Limit' */
6432 break;
6433
6434 case motor_IN_Int2_i:
6435 /* During 'Int2': '<S1>:261' */
6436 if (motor_DWork.temporalCounter_i1 >= 50U) {
6437 /* Transition: '<S1>:697' */
6438 motor_DWork.is_XHZY_a = motor_IN_Int7;
6439 motor_DWork.temporalCounter_i1 = 0U;
6440 } else {
6441 motor_DWork.chu_jd += 0.002;
6442
6443 /* Inport: '<Root>/JD_In' */
6444 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
6445 motor_B.JD_In_d = motor_U.JD_In;
6446 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
6447
6448 /* Inport: '<Root>/YJ_In' */
6449 motor_B.YJ_In = motor_U.YJ_In;
6450
6451 /* Inport: '<Root>/Encode_Sp' */
6452 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
6453
6454 /* Inport: '<Root>/System_Order' */
6455 motor_B.Slect_port_h = motor_U.System_Order;
6456
6457 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
6458 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
6459 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
6460 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
6461 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
6462 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6463 &motor_DWork.EN_extern, &motor_DWork.Forword,
6464 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
6465 &motor_DWork.KG_En, &motor_DWork.KG_JD,
6466 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
6467 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6468 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
6469 &motor_DWork.chu_jd);
6470
6471 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
6472 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
6473 if (tmp_1 < 65536.0) {
6474 if (tmp_1 >= 0.0) {
6475 tmp_0 = (uint16_T)tmp_1;
6476 } else {
6477 tmp_0 = 0U;
6478 }
6479 } else {
6480 tmp_0 = MAX_uint16_T;
6481 }
6482
6483 motor_Y.PWMOUT = tmp_0;
6484 tmp_1 = motor_B.Motor_XHZY.KP_JD;
6485 if (tmp_1 < 2.147483648E+9) {
6486 if (tmp_1 >= -2.147483648E+9) {
6487 tmp = (int32_T)tmp_1;
6488 } else {
6489 tmp = MIN_int32_T;
6490 }
6491 } else {
6492 tmp = MAX_int32_T;
6493 }
6494
6495 motor_Y.JD_Out = tmp;
6496 tmp_1 = motor_B.Motor_XHZY.KP_e;
6497 if (tmp_1 < 2.147483648E+9) {
6498 if (tmp_1 >= -2.147483648E+9) {
6499 tmp = (int32_T)tmp_1;
6500 } else {
6501 tmp = MIN_int32_T;
6502 }
6503 } else {
6504 tmp = MAX_int32_T;
6505 }
6506
6507 motor_Y.JD_Error = tmp;
6508 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
6509 }
6510 break;
6511
6512 case motor_IN_Int3_l:
6513 /* During 'Int3': '<S1>:262' */
6514 if (fabs(motor_DWork.chu_jd) < 0.1) {
6515 /* Transition: '<S1>:698' */
6516 motor_DWork.is_XHZY_a = motor_IN_Int4_b;
6517 motor_DWork.temporalCounter_i1 = 0U;
6518
6519 /* Entry 'Int4': '<S1>:263' */
6520 motor_DWork.chu_jd = 0.0;
6521
6522 /* Inport: '<Root>/JD_In' */
6523 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
6524 motor_B.JD_In_d = motor_U.JD_In;
6525 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
6526
6527 /* Inport: '<Root>/YJ_In' */
6528 motor_B.YJ_In = motor_U.YJ_In;
6529
6530 /* Inport: '<Root>/Encode_Sp' */
6531 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
6532
6533 /* Inport: '<Root>/System_Order' */
6534 motor_B.Slect_port_h = motor_U.System_Order;
6535
6536 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
6537 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
6538 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
6539 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
6540 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
6541 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6542 &motor_DWork.EN_extern, &motor_DWork.Forword,
6543 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
6544 &motor_DWork.KG_En, &motor_DWork.KG_JD,
6545 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
6546 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6547 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
6548 &motor_DWork.chu_jd);
6549
6550 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
6551 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
6552 if (tmp_1 < 65536.0) {
6553 if (tmp_1 >= 0.0) {
6554 tmp_0 = (uint16_T)tmp_1;
6555 } else {
6556 tmp_0 = 0U;
6557 }
6558 } else {
6559 tmp_0 = MAX_uint16_T;
6560 }
6561
6562 motor_Y.PWMOUT = tmp_0;
6563 tmp_1 = motor_B.Motor_XHZY.KP_JD;
6564 if (tmp_1 < 2.147483648E+9) {
6565 if (tmp_1 >= -2.147483648E+9) {
6566 tmp = (int32_T)tmp_1;
6567 } else {
6568 tmp = MIN_int32_T;
6569 }
6570 } else {
6571 tmp = MAX_int32_T;
6572 }
6573
6574 motor_Y.JD_Out = tmp;
6575 tmp_1 = motor_B.Motor_XHZY.KP_e;
6576 if (tmp_1 < 2.147483648E+9) {
6577 if (tmp_1 >= -2.147483648E+9) {
6578 tmp = (int32_T)tmp_1;
6579 } else {
6580 tmp = MIN_int32_T;
6581 }
6582 } else {
6583 tmp = MAX_int32_T;
6584 }
6585
6586 motor_Y.JD_Error = tmp;
6587 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
6588 } else {
6589 motor_DWork.chu_jd -= 0.01;
6590
6591 /* Inport: '<Root>/JD_In' */
6592 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
6593 motor_B.JD_In_d = motor_U.JD_In;
6594 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
6595
6596 /* Inport: '<Root>/YJ_In' */
6597 motor_B.YJ_In = motor_U.YJ_In;
6598
6599 /* Inport: '<Root>/Encode_Sp' */
6600 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
6601
6602 /* Inport: '<Root>/System_Order' */
6603 motor_B.Slect_port_h = motor_U.System_Order;
6604
6605 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
6606 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
6607 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
6608 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
6609 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
6610 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6611 &motor_DWork.EN_extern, &motor_DWork.Forword,
6612 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
6613 &motor_DWork.KG_En, &motor_DWork.KG_JD,
6614 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
6615 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6616 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
6617 &motor_DWork.chu_jd);
6618
6619 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
6620 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
6621 if (tmp_1 < 65536.0) {
6622 if (tmp_1 >= 0.0) {
6623 tmp_0 = (uint16_T)tmp_1;
6624 } else {
6625 tmp_0 = 0U;
6626 }
6627 } else {
6628 tmp_0 = MAX_uint16_T;
6629 }
6630
6631 motor_Y.PWMOUT = tmp_0;
6632 tmp_1 = motor_B.Motor_XHZY.KP_JD;
6633 if (tmp_1 < 2.147483648E+9) {
6634 if (tmp_1 >= -2.147483648E+9) {
6635 tmp = (int32_T)tmp_1;
6636 } else {
6637 tmp = MIN_int32_T;
6638 }
6639 } else {
6640 tmp = MAX_int32_T;
6641 }
6642
6643 motor_Y.JD_Out = tmp;
6644 tmp_1 = motor_B.Motor_XHZY.KP_e;
6645 if (tmp_1 < 2.147483648E+9) {
6646 if (tmp_1 >= -2.147483648E+9) {
6647 tmp = (int32_T)tmp_1;
6648 } else {
6649 tmp = MIN_int32_T;
6650 }
6651 } else {
6652 tmp = MAX_int32_T;
6653 }
6654
6655 motor_Y.JD_Error = tmp;
6656 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
6657 }
6658 break;
6659
6660 case motor_IN_Int4_b:
6661 /* During 'Int4': '<S1>:263' */
6662 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
6663 /* Transition: '<S1>:700' */
6664 motor_DWork.is_XHZY_a = motor_IN_Int5_a;
6665 motor_DWork.temporalCounter_i1 = 0U;
6666
6667 /* Entry 'Int5': '<S1>:264' */
6668 motor_Y.Motor_En = true;
6669
6670 /* 电机失能 */
6671 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
6672 } else {
6673 /* Inport: '<Root>/JD_In' */
6674 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
6675 motor_B.JD_In_d = motor_U.JD_In;
6676 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
6677
6678 /* Inport: '<Root>/YJ_In' */
6679 motor_B.YJ_In = motor_U.YJ_In;
6680
6681 /* Inport: '<Root>/Encode_Sp' */
6682 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
6683
6684 /* Inport: '<Root>/System_Order' */
6685 motor_B.Slect_port_h = motor_U.System_Order;
6686
6687 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
6688 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
6689 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
6690 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
6691 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
6692 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6693 &motor_DWork.EN_extern, &motor_DWork.Forword,
6694 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
6695 &motor_DWork.KG_En, &motor_DWork.KG_JD,
6696 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
6697 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6698 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
6699 &motor_DWork.chu_jd);
6700
6701 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
6702 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
6703 if (tmp_1 < 65536.0) {
6704 if (tmp_1 >= 0.0) {
6705 tmp_0 = (uint16_T)tmp_1;
6706 } else {
6707 tmp_0 = 0U;
6708 }
6709 } else {
6710 tmp_0 = MAX_uint16_T;
6711 }
6712
6713 motor_Y.PWMOUT = tmp_0;
6714 tmp_1 = motor_B.Motor_XHZY.KP_JD;
6715 if (tmp_1 < 2.147483648E+9) {
6716 if (tmp_1 >= -2.147483648E+9) {
6717 tmp = (int32_T)tmp_1;
6718 } else {
6719 tmp = MIN_int32_T;
6720 }
6721 } else {
6722 tmp = MAX_int32_T;
6723 }
6724
6725 motor_Y.JD_Out = tmp;
6726 tmp_1 = motor_B.Motor_XHZY.KP_e;
6727 if (tmp_1 < 2.147483648E+9) {
6728 if (tmp_1 >= -2.147483648E+9) {
6729 tmp = (int32_T)tmp_1;
6730 } else {
6731 tmp = MIN_int32_T;
6732 }
6733 } else {
6734 tmp = MAX_int32_T;
6735 }
6736
6737 motor_Y.JD_Error = tmp;
6738 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
6739 }
6740 break;
6741
6742 case motor_IN_Int5_a:
6743 /* During 'Int5': '<S1>:264' */
6744 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
6745 /* Transition: '<S1>:699' */
6746 motor_DWork.is_XHZY_a = motor_IN_Int6_m;
6747
6748 /* Entry 'Int6': '<S1>:265' */
6749 motor_Y.DCZD = true;
6750
6751 /* 开启制动 */
6752 }
6753 break;
6754
6755 case motor_IN_Int6_m:
6756 /* During 'Int6': '<S1>:265' */
6757 /* Transition: '<S1>:696' */
6758 /* Transition: '<S1>:663' */
6759 motor_DWork.is_XHZY_a = motor_IN_NO_ACTIVE_CHILD;
6760 motor_DWork.is_Limit_Up_Test = motor_IN_NO_ACTIVE_CHILD;
6761 motor_DWork.is_Test_Mode = motor_IN_defult;
6762
6763 /* Entry 'defult': '<S1>:239' */
6764 motor_Y.Open_Result = 1U;
6765
6766 /* 开机状态位成功 */
6767 motor_DWork.In_State = 2U;
6768 break;
6769
6770 case motor_IN_Int7:
6771 /* During 'Int7': '<S1>:967' */
6772 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
6773 /* Transition: '<S1>:968' */
6774 motor_DWork.is_XHZY_a = motor_IN_Int3_l;
6775
6776 /* Entry 'Int3': '<S1>:262' */
6777 motor_DWork.chu_jd -= 0.01;
6778
6779 /* Inport: '<Root>/JD_In' */
6780 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
6781 motor_B.JD_In_d = motor_U.JD_In;
6782 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
6783
6784 /* Inport: '<Root>/YJ_In' */
6785 motor_B.YJ_In = motor_U.YJ_In;
6786
6787 /* Inport: '<Root>/Encode_Sp' */
6788 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
6789
6790 /* Inport: '<Root>/System_Order' */
6791 motor_B.Slect_port_h = motor_U.System_Order;
6792
6793 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
6794 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
6795 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
6796 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
6797 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
6798 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6799 &motor_DWork.EN_extern, &motor_DWork.Forword,
6800 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
6801 &motor_DWork.KG_En, &motor_DWork.KG_JD,
6802 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
6803 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6804 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
6805 &motor_DWork.chu_jd);
6806
6807 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
6808 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
6809 if (tmp_1 < 65536.0) {
6810 if (tmp_1 >= 0.0) {
6811 tmp_0 = (uint16_T)tmp_1;
6812 } else {
6813 tmp_0 = 0U;
6814 }
6815 } else {
6816 tmp_0 = MAX_uint16_T;
6817 }
6818
6819 motor_Y.PWMOUT = tmp_0;
6820 tmp_1 = motor_B.Motor_XHZY.KP_JD;
6821 if (tmp_1 < 2.147483648E+9) {
6822 if (tmp_1 >= -2.147483648E+9) {
6823 tmp = (int32_T)tmp_1;
6824 } else {
6825 tmp = MIN_int32_T;
6826 }
6827 } else {
6828 tmp = MAX_int32_T;
6829 }
6830
6831 motor_Y.JD_Out = tmp;
6832 tmp_1 = motor_B.Motor_XHZY.KP_e;
6833 if (tmp_1 < 2.147483648E+9) {
6834 if (tmp_1 >= -2.147483648E+9) {
6835 tmp = (int32_T)tmp_1;
6836 } else {
6837 tmp = MIN_int32_T;
6838 }
6839 } else {
6840 tmp = MAX_int32_T;
6841 }
6842
6843 motor_Y.JD_Error = tmp;
6844 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
6845 } else {
6846 /* Inport: '<Root>/JD_In' */
6847 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
6848 motor_B.JD_In_d = motor_U.JD_In;
6849 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
6850
6851 /* Inport: '<Root>/YJ_In' */
6852 motor_B.YJ_In = motor_U.YJ_In;
6853
6854 /* Inport: '<Root>/Encode_Sp' */
6855 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
6856
6857 /* Inport: '<Root>/System_Order' */
6858 motor_B.Slect_port_h = motor_U.System_Order;
6859
6860 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
6861 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
6862 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
6863 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
6864 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
6865 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6866 &motor_DWork.EN_extern, &motor_DWork.Forword,
6867 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
6868 &motor_DWork.KG_En, &motor_DWork.KG_JD,
6869 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
6870 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6871 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
6872 &motor_DWork.chu_jd);
6873
6874 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
6875 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
6876 if (tmp_1 < 65536.0) {
6877 if (tmp_1 >= 0.0) {
6878 tmp_0 = (uint16_T)tmp_1;
6879 } else {
6880 tmp_0 = 0U;
6881 }
6882 } else {
6883 tmp_0 = MAX_uint16_T;
6884 }
6885
6886 motor_Y.PWMOUT = tmp_0;
6887 tmp_1 = motor_B.Motor_XHZY.KP_JD;
6888 if (tmp_1 < 2.147483648E+9) {
6889 if (tmp_1 >= -2.147483648E+9) {
6890 tmp = (int32_T)tmp_1;
6891 } else {
6892 tmp = MIN_int32_T;
6893 }
6894 } else {
6895 tmp = MAX_int32_T;
6896 }
6897
6898 motor_Y.JD_Out = tmp;
6899 tmp_1 = motor_B.Motor_XHZY.KP_e;
6900 if (tmp_1 < 2.147483648E+9) {
6901 if (tmp_1 >= -2.147483648E+9) {
6902 tmp = (int32_T)tmp_1;
6903 } else {
6904 tmp = MIN_int32_T;
6905 }
6906 } else {
6907 tmp = MAX_int32_T;
6908 }
6909
6910 motor_Y.JD_Error = tmp;
6911 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
6912 }
6913 break;
6914
6915 default:
6916 /* During 'Int8': '<S1>:986' */
6917 if (motor_DWork.chu_jd > 16.0) {
6918 /* Transition: '<S1>:987' */
6919 motor_DWork.is_XHZY_a = motor_IN_Int1;
6920 motor_DWork.temporalCounter_i1 = 0U;
6921
6922 /* Entry 'Int1': '<S1>:260' */
6923 motor_DWork.chu_jd += 0.002;
6924
6925 /* Inport: '<Root>/JD_In' */
6926 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
6927 motor_B.JD_In_d = motor_U.JD_In;
6928 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
6929
6930 /* Inport: '<Root>/YJ_In' */
6931 motor_B.YJ_In = motor_U.YJ_In;
6932
6933 /* Inport: '<Root>/Encode_Sp' */
6934 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
6935
6936 /* Inport: '<Root>/System_Order' */
6937 motor_B.Slect_port_h = motor_U.System_Order;
6938
6939 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
6940 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
6941 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
6942 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
6943 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
6944 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
6945 &motor_DWork.EN_extern, &motor_DWork.Forword,
6946 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
6947 &motor_DWork.KG_En, &motor_DWork.KG_JD,
6948 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
6949 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
6950 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
6951 &motor_DWork.chu_jd);
6952
6953 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
6954 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
6955 if (tmp_1 < 65536.0) {
6956 if (tmp_1 >= 0.0) {
6957 tmp_0 = (uint16_T)tmp_1;
6958 } else {
6959 tmp_0 = 0U;
6960 }
6961 } else {
6962 tmp_0 = MAX_uint16_T;
6963 }
6964
6965 motor_Y.PWMOUT = tmp_0;
6966 tmp_1 = motor_B.Motor_XHZY.KP_JD;
6967 if (tmp_1 < 2.147483648E+9) {
6968 if (tmp_1 >= -2.147483648E+9) {
6969 tmp = (int32_T)tmp_1;
6970 } else {
6971 tmp = MIN_int32_T;
6972 }
6973 } else {
6974 tmp = MAX_int32_T;
6975 }
6976
6977 motor_Y.JD_Out = tmp;
6978 tmp_1 = motor_B.Motor_XHZY.KP_e;
6979 if (tmp_1 < 2.147483648E+9) {
6980 if (tmp_1 >= -2.147483648E+9) {
6981 tmp = (int32_T)tmp_1;
6982 } else {
6983 tmp = MIN_int32_T;
6984 }
6985 } else {
6986 tmp = MAX_int32_T;
6987 }
6988
6989 motor_Y.JD_Error = tmp;
6990 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
6991 } else if (motor_U.Up_Limit == 0) {
6992 /* Transition: '<S1>:1002' */
6993 /* 上限位开关低电平有效 */
6994 motor_DWork.is_XHZY_a = motor_IN_Int2_i;
6995 motor_DWork.temporalCounter_i1 = 0U;
6996
6997 /* Entry 'Int2': '<S1>:261' */
6998 motor_DWork.chu_jd += 0.002;
6999
7000 /* Inport: '<Root>/JD_In' */
7001 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
7002 motor_B.JD_In_d = motor_U.JD_In;
7003 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
7004
7005 /* Inport: '<Root>/YJ_In' */
7006 motor_B.YJ_In = motor_U.YJ_In;
7007
7008 /* Inport: '<Root>/Encode_Sp' */
7009 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
7010
7011 /* Inport: '<Root>/System_Order' */
7012 motor_B.Slect_port_h = motor_U.System_Order;
7013
7014 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
7015 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
7016 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
7017 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
7018 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
7019 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
7020 &motor_DWork.EN_extern, &motor_DWork.Forword,
7021 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
7022 &motor_DWork.KG_En, &motor_DWork.KG_JD,
7023 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
7024 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
7025 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
7026 &motor_DWork.chu_jd);
7027
7028 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
7029 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
7030 if (tmp_1 < 65536.0) {
7031 if (tmp_1 >= 0.0) {
7032 tmp_0 = (uint16_T)tmp_1;
7033 } else {
7034 tmp_0 = 0U;
7035 }
7036 } else {
7037 tmp_0 = MAX_uint16_T;
7038 }
7039
7040 motor_Y.PWMOUT = tmp_0;
7041 tmp_1 = motor_B.Motor_XHZY.KP_JD;
7042 if (tmp_1 < 2.147483648E+9) {
7043 if (tmp_1 >= -2.147483648E+9) {
7044 tmp = (int32_T)tmp_1;
7045 } else {
7046 tmp = MIN_int32_T;
7047 }
7048 } else {
7049 tmp = MAX_int32_T;
7050 }
7051
7052 motor_Y.JD_Out = tmp;
7053 tmp_1 = motor_B.Motor_XHZY.KP_e;
7054 if (tmp_1 < 2.147483648E+9) {
7055 if (tmp_1 >= -2.147483648E+9) {
7056 tmp = (int32_T)tmp_1;
7057 } else {
7058 tmp = MIN_int32_T;
7059 }
7060 } else {
7061 tmp = MAX_int32_T;
7062 }
7063
7064 motor_Y.JD_Error = tmp;
7065 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
7066 } else {
7067 motor_DWork.chu_jd += 0.01;
7068
7069 /* Inport: '<Root>/JD_In' */
7070 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
7071 motor_B.JD_In_d = motor_U.JD_In;
7072 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
7073
7074 /* Inport: '<Root>/YJ_In' */
7075 motor_B.YJ_In = motor_U.YJ_In;
7076
7077 /* Inport: '<Root>/Encode_Sp' */
7078 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
7079
7080 /* Inport: '<Root>/System_Order' */
7081 motor_B.Slect_port_h = motor_U.System_Order;
7082
7083 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
7084 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
7085 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
7086 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
7087 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
7088 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
7089 &motor_DWork.EN_extern, &motor_DWork.Forword,
7090 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
7091 &motor_DWork.KG_En, &motor_DWork.KG_JD,
7092 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
7093 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
7094 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
7095 &motor_DWork.chu_jd);
7096
7097 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
7098 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
7099 if (tmp_1 < 65536.0) {
7100 if (tmp_1 >= 0.0) {
7101 tmp_0 = (uint16_T)tmp_1;
7102 } else {
7103 tmp_0 = 0U;
7104 }
7105 } else {
7106 tmp_0 = MAX_uint16_T;
7107 }
7108
7109 motor_Y.PWMOUT = tmp_0;
7110 tmp_1 = motor_B.Motor_XHZY.KP_JD;
7111 if (tmp_1 < 2.147483648E+9) {
7112 if (tmp_1 >= -2.147483648E+9) {
7113 tmp = (int32_T)tmp_1;
7114 } else {
7115 tmp = MIN_int32_T;
7116 }
7117 } else {
7118 tmp = MAX_int32_T;
7119 }
7120
7121 motor_Y.JD_Out = tmp;
7122 tmp_1 = motor_B.Motor_XHZY.KP_e;
7123 if (tmp_1 < 2.147483648E+9) {
7124 if (tmp_1 >= -2.147483648E+9) {
7125 tmp = (int32_T)tmp_1;
7126 } else {
7127 tmp = MIN_int32_T;
7128 }
7129 } else {
7130 tmp = MAX_int32_T;
7131 }
7132
7133 motor_Y.JD_Error = tmp;
7134 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
7135 }
7136 break;
7137 }
7138}
7139
7140/* Function for Chart: '<Root>/Chart' */
7141static void motor_Limit_Up_Test(void)
7142{
7143 int32_T tmp;
7144 real_T tmp_0;
7145 uint16_T tmp_1;
7146
7147 /* During 'Limit_Up_Test': '<S1>:238' */
7148 switch (motor_DWork.is_Limit_Up_Test) {
7149 case motor_IN_HY_f:
7150 /* During 'HY': '<S1>:258' */
7151 switch (motor_DWork.is_HY_a) {
7152 case motor_IN_Int:
7153 /* During 'Int': '<S1>:275' */
7154 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
7155 /* Transition: '<S1>:702' */
7156 motor_DWork.is_HY_a = motor_IN_Int8;
7157 }
7158 break;
7159
7160 case motor_IN_Int1:
7161 /* Inport: '<Root>/Up_Limit' */
7162 /* During 'Int1': '<S1>:278' */
7163 if (motor_U.Up_Limit == 0) {
7164 /* Transition: '<S1>:703' */
7165 /* 上限位开关低电平有效 */
7166 motor_DWork.is_HY_a = motor_IN_Int2_i;
7167 motor_DWork.temporalCounter_i1 = 0U;
7168
7169 /* Entry 'Int2': '<S1>:279' */
7170 motor_DWork.chu_jd += 0.002;
7171
7172 /* Inport: '<Root>/JD_In' */
7173 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7174 motor_B.JD_In_f = motor_U.JD_In;
7175 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7176
7177 /* Inport: '<Root>/System_Order' */
7178 motor_B.Slect_port_c = motor_U.System_Order;
7179
7180 /* Inport: '<Root>/Encode_Sp' */
7181 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7182
7183 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7184 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7185 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7186 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7187 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7188 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7189 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7190 &motor_DWork.chu_jd);
7191
7192 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7193 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7194 tmp_0 = motor_B.Motor_HYDG.KP_e;
7195 if (tmp_0 < 2.147483648E+9) {
7196 if (tmp_0 >= -2.147483648E+9) {
7197 tmp = (int32_T)tmp_0;
7198 } else {
7199 tmp = MIN_int32_T;
7200 }
7201 } else {
7202 tmp = MAX_int32_T;
7203 }
7204
7205 motor_Y.JD_Error = tmp;
7206 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7207 if (tmp_0 < 2.147483648E+9) {
7208 if (tmp_0 >= -2.147483648E+9) {
7209 tmp = (int32_T)tmp_0;
7210 } else {
7211 tmp = MIN_int32_T;
7212 }
7213 } else {
7214 tmp = MAX_int32_T;
7215 }
7216
7217 motor_Y.JD_Out = tmp;
7218 } else if (motor_DWork.temporalCounter_i1 >= (uint32_T)(7.0 /
7219 motor_DWork.Ts)) {
7220 /* Transition: '<S1>:1037' */
7221 motor_Y.Flag_Up_GZ_limit = 0U;
7222
7223 /* 上限位开关故障$/ */
7224 motor_DWork.is_HY_a = motor_IN_Int2_i;
7225 motor_DWork.temporalCounter_i1 = 0U;
7226
7227 /* Entry 'Int2': '<S1>:279' */
7228 motor_DWork.chu_jd += 0.002;
7229
7230 /* Inport: '<Root>/JD_In' */
7231 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7232 motor_B.JD_In_f = motor_U.JD_In;
7233 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7234
7235 /* Inport: '<Root>/System_Order' */
7236 motor_B.Slect_port_c = motor_U.System_Order;
7237
7238 /* Inport: '<Root>/Encode_Sp' */
7239 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7240
7241 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7242 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7243 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7244 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7245 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7246 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7247 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7248 &motor_DWork.chu_jd);
7249
7250 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7251 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7252 tmp_0 = motor_B.Motor_HYDG.KP_e;
7253 if (tmp_0 < 2.147483648E+9) {
7254 if (tmp_0 >= -2.147483648E+9) {
7255 tmp = (int32_T)tmp_0;
7256 } else {
7257 tmp = MIN_int32_T;
7258 }
7259 } else {
7260 tmp = MAX_int32_T;
7261 }
7262
7263 motor_Y.JD_Error = tmp;
7264 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7265 if (tmp_0 < 2.147483648E+9) {
7266 if (tmp_0 >= -2.147483648E+9) {
7267 tmp = (int32_T)tmp_0;
7268 } else {
7269 tmp = MIN_int32_T;
7270 }
7271 } else {
7272 tmp = MAX_int32_T;
7273 }
7274
7275 motor_Y.JD_Out = tmp;
7276 } else {
7277 motor_DWork.chu_jd += 0.002;
7278
7279 /* Inport: '<Root>/JD_In' */
7280 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7281 motor_B.JD_In_f = motor_U.JD_In;
7282 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7283
7284 /* Inport: '<Root>/System_Order' */
7285 motor_B.Slect_port_c = motor_U.System_Order;
7286
7287 /* Inport: '<Root>/Encode_Sp' */
7288 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7289
7290 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7291 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7292 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7293 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7294 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7295 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7296 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7297 &motor_DWork.chu_jd);
7298
7299 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7300 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7301 tmp_0 = motor_B.Motor_HYDG.KP_e;
7302 if (tmp_0 < 2.147483648E+9) {
7303 if (tmp_0 >= -2.147483648E+9) {
7304 tmp = (int32_T)tmp_0;
7305 } else {
7306 tmp = MIN_int32_T;
7307 }
7308 } else {
7309 tmp = MAX_int32_T;
7310 }
7311
7312 motor_Y.JD_Error = tmp;
7313 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7314 if (tmp_0 < 2.147483648E+9) {
7315 if (tmp_0 >= -2.147483648E+9) {
7316 tmp = (int32_T)tmp_0;
7317 } else {
7318 tmp = MIN_int32_T;
7319 }
7320 } else {
7321 tmp = MAX_int32_T;
7322 }
7323
7324 motor_Y.JD_Out = tmp;
7325 }
7326 break;
7327
7328 case motor_IN_Int2_i:
7329 /* During 'Int2': '<S1>:279' */
7330 if (motor_DWork.temporalCounter_i1 >= 50U) {
7331 /* Transition: '<S1>:705' */
7332 motor_DWork.is_HY_a = motor_IN_Int7;
7333 motor_DWork.temporalCounter_i1 = 0U;
7334 } else {
7335 motor_DWork.chu_jd += 0.002;
7336
7337 /* Inport: '<Root>/JD_In' */
7338 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7339 motor_B.JD_In_f = motor_U.JD_In;
7340 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7341
7342 /* Inport: '<Root>/System_Order' */
7343 motor_B.Slect_port_c = motor_U.System_Order;
7344
7345 /* Inport: '<Root>/Encode_Sp' */
7346 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7347
7348 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7349 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7350 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7351 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7352 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7353 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7354 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7355 &motor_DWork.chu_jd);
7356
7357 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7358 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7359 tmp_0 = motor_B.Motor_HYDG.KP_e;
7360 if (tmp_0 < 2.147483648E+9) {
7361 if (tmp_0 >= -2.147483648E+9) {
7362 tmp = (int32_T)tmp_0;
7363 } else {
7364 tmp = MIN_int32_T;
7365 }
7366 } else {
7367 tmp = MAX_int32_T;
7368 }
7369
7370 motor_Y.JD_Error = tmp;
7371 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7372 if (tmp_0 < 2.147483648E+9) {
7373 if (tmp_0 >= -2.147483648E+9) {
7374 tmp = (int32_T)tmp_0;
7375 } else {
7376 tmp = MIN_int32_T;
7377 }
7378 } else {
7379 tmp = MAX_int32_T;
7380 }
7381
7382 motor_Y.JD_Out = tmp;
7383 }
7384 break;
7385
7386 case motor_IN_Int3_l:
7387 /* During 'Int3': '<S1>:274' */
7388 if (fabs(motor_DWork.chu_jd) < 0.1) {
7389 /* Transition: '<S1>:706' */
7390 motor_DWork.is_HY_a = motor_IN_Int4_b;
7391 motor_DWork.temporalCounter_i1 = 0U;
7392
7393 /* Entry 'Int4': '<S1>:277' */
7394 motor_DWork.chu_jd = 0.0;
7395
7396 /* Inport: '<Root>/JD_In' */
7397 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7398 motor_B.JD_In_f = motor_U.JD_In;
7399 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7400
7401 /* Inport: '<Root>/System_Order' */
7402 motor_B.Slect_port_c = motor_U.System_Order;
7403
7404 /* Inport: '<Root>/Encode_Sp' */
7405 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7406
7407 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7408 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7409 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7410 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7411 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7412 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7413 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7414 &motor_DWork.chu_jd);
7415
7416 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7417 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7418 tmp_0 = motor_B.Motor_HYDG.KP_e;
7419 if (tmp_0 < 2.147483648E+9) {
7420 if (tmp_0 >= -2.147483648E+9) {
7421 tmp = (int32_T)tmp_0;
7422 } else {
7423 tmp = MIN_int32_T;
7424 }
7425 } else {
7426 tmp = MAX_int32_T;
7427 }
7428
7429 motor_Y.JD_Error = tmp;
7430 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7431 if (tmp_0 < 2.147483648E+9) {
7432 if (tmp_0 >= -2.147483648E+9) {
7433 tmp = (int32_T)tmp_0;
7434 } else {
7435 tmp = MIN_int32_T;
7436 }
7437 } else {
7438 tmp = MAX_int32_T;
7439 }
7440
7441 motor_Y.JD_Out = tmp;
7442 } else {
7443 motor_DWork.chu_jd -= 0.01;
7444
7445 /* Inport: '<Root>/JD_In' */
7446 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7447 motor_B.JD_In_f = motor_U.JD_In;
7448 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7449
7450 /* Inport: '<Root>/System_Order' */
7451 motor_B.Slect_port_c = motor_U.System_Order;
7452
7453 /* Inport: '<Root>/Encode_Sp' */
7454 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7455
7456 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7457 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7458 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7459 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7460 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7461 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7462 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7463 &motor_DWork.chu_jd);
7464
7465 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7466 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7467 tmp_0 = motor_B.Motor_HYDG.KP_e;
7468 if (tmp_0 < 2.147483648E+9) {
7469 if (tmp_0 >= -2.147483648E+9) {
7470 tmp = (int32_T)tmp_0;
7471 } else {
7472 tmp = MIN_int32_T;
7473 }
7474 } else {
7475 tmp = MAX_int32_T;
7476 }
7477
7478 motor_Y.JD_Error = tmp;
7479 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7480 if (tmp_0 < 2.147483648E+9) {
7481 if (tmp_0 >= -2.147483648E+9) {
7482 tmp = (int32_T)tmp_0;
7483 } else {
7484 tmp = MIN_int32_T;
7485 }
7486 } else {
7487 tmp = MAX_int32_T;
7488 }
7489
7490 motor_Y.JD_Out = tmp;
7491 }
7492 break;
7493
7494 case motor_IN_Int4_b:
7495 /* During 'Int4': '<S1>:277' */
7496 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
7497 /* Transition: '<S1>:708' */
7498 motor_DWork.is_HY_a = motor_IN_Int5_a;
7499 motor_DWork.temporalCounter_i1 = 0U;
7500
7501 /* Entry 'Int5': '<S1>:276' */
7502 motor_Y.Motor_En = true;
7503
7504 /* 电机失能 */
7505 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
7506 } else {
7507 /* Inport: '<Root>/JD_In' */
7508 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7509 motor_B.JD_In_f = motor_U.JD_In;
7510 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7511
7512 /* Inport: '<Root>/System_Order' */
7513 motor_B.Slect_port_c = motor_U.System_Order;
7514
7515 /* Inport: '<Root>/Encode_Sp' */
7516 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7517
7518 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7519 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7520 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7521 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7522 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7523 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7524 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7525 &motor_DWork.chu_jd);
7526
7527 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7528 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7529 tmp_0 = motor_B.Motor_HYDG.KP_e;
7530 if (tmp_0 < 2.147483648E+9) {
7531 if (tmp_0 >= -2.147483648E+9) {
7532 tmp = (int32_T)tmp_0;
7533 } else {
7534 tmp = MIN_int32_T;
7535 }
7536 } else {
7537 tmp = MAX_int32_T;
7538 }
7539
7540 motor_Y.JD_Error = tmp;
7541 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7542 if (tmp_0 < 2.147483648E+9) {
7543 if (tmp_0 >= -2.147483648E+9) {
7544 tmp = (int32_T)tmp_0;
7545 } else {
7546 tmp = MIN_int32_T;
7547 }
7548 } else {
7549 tmp = MAX_int32_T;
7550 }
7551
7552 motor_Y.JD_Out = tmp;
7553 }
7554 break;
7555
7556 case motor_IN_Int5_a:
7557 /* During 'Int5': '<S1>:276' */
7558 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
7559 /* Transition: '<S1>:707' */
7560 motor_DWork.is_HY_a = motor_IN_Int6_m;
7561
7562 /* Entry 'Int6': '<S1>:273' */
7563 motor_Y.DCZD = true;
7564
7565 /* 开启制动 */
7566 }
7567 break;
7568
7569 case motor_IN_Int6_m:
7570 /* During 'Int6': '<S1>:273' */
7571 /* Transition: '<S1>:704' */
7572 /* Transition: '<S1>:663' */
7573 motor_DWork.is_HY_a = motor_IN_NO_ACTIVE_CHILD;
7574 motor_DWork.is_Limit_Up_Test = motor_IN_NO_ACTIVE_CHILD;
7575 motor_DWork.is_Test_Mode = motor_IN_defult;
7576
7577 /* Entry 'defult': '<S1>:239' */
7578 motor_Y.Open_Result = 1U;
7579
7580 /* 开机状态位成功 */
7581 motor_DWork.In_State = 2U;
7582 break;
7583
7584 case motor_IN_Int7:
7585 /* During 'Int7': '<S1>:969' */
7586 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
7587 /* Transition: '<S1>:970' */
7588 motor_DWork.is_HY_a = motor_IN_Int3_l;
7589
7590 /* Entry 'Int3': '<S1>:274' */
7591 motor_DWork.chu_jd -= 0.01;
7592
7593 /* Inport: '<Root>/JD_In' */
7594 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7595 motor_B.JD_In_f = motor_U.JD_In;
7596 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7597
7598 /* Inport: '<Root>/System_Order' */
7599 motor_B.Slect_port_c = motor_U.System_Order;
7600
7601 /* Inport: '<Root>/Encode_Sp' */
7602 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7603
7604 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7605 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7606 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7607 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7608 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7609 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7610 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7611 &motor_DWork.chu_jd);
7612
7613 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7614 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7615 tmp_0 = motor_B.Motor_HYDG.KP_e;
7616 if (tmp_0 < 2.147483648E+9) {
7617 if (tmp_0 >= -2.147483648E+9) {
7618 tmp = (int32_T)tmp_0;
7619 } else {
7620 tmp = MIN_int32_T;
7621 }
7622 } else {
7623 tmp = MAX_int32_T;
7624 }
7625
7626 motor_Y.JD_Error = tmp;
7627 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7628 if (tmp_0 < 2.147483648E+9) {
7629 if (tmp_0 >= -2.147483648E+9) {
7630 tmp = (int32_T)tmp_0;
7631 } else {
7632 tmp = MIN_int32_T;
7633 }
7634 } else {
7635 tmp = MAX_int32_T;
7636 }
7637
7638 motor_Y.JD_Out = tmp;
7639 } else {
7640 /* Inport: '<Root>/JD_In' */
7641 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7642 motor_B.JD_In_f = motor_U.JD_In;
7643 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7644
7645 /* Inport: '<Root>/System_Order' */
7646 motor_B.Slect_port_c = motor_U.System_Order;
7647
7648 /* Inport: '<Root>/Encode_Sp' */
7649 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7650
7651 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7652 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7653 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7654 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7655 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7656 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7657 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7658 &motor_DWork.chu_jd);
7659
7660 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7661 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7662 tmp_0 = motor_B.Motor_HYDG.KP_e;
7663 if (tmp_0 < 2.147483648E+9) {
7664 if (tmp_0 >= -2.147483648E+9) {
7665 tmp = (int32_T)tmp_0;
7666 } else {
7667 tmp = MIN_int32_T;
7668 }
7669 } else {
7670 tmp = MAX_int32_T;
7671 }
7672
7673 motor_Y.JD_Error = tmp;
7674 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7675 if (tmp_0 < 2.147483648E+9) {
7676 if (tmp_0 >= -2.147483648E+9) {
7677 tmp = (int32_T)tmp_0;
7678 } else {
7679 tmp = MIN_int32_T;
7680 }
7681 } else {
7682 tmp = MAX_int32_T;
7683 }
7684
7685 motor_Y.JD_Out = tmp;
7686 }
7687 break;
7688
7689 default:
7690 /* During 'Int8': '<S1>:990' */
7691 if (motor_DWork.chu_jd > 14.0) {
7692 /* Transition: '<S1>:991' */
7693 motor_DWork.is_HY_a = motor_IN_Int1;
7694 motor_DWork.temporalCounter_i1 = 0U;
7695
7696 /* Entry 'Int1': '<S1>:278' */
7697 motor_DWork.chu_jd += 0.002;
7698
7699 /* Inport: '<Root>/JD_In' */
7700 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7701 motor_B.JD_In_f = motor_U.JD_In;
7702 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7703
7704 /* Inport: '<Root>/System_Order' */
7705 motor_B.Slect_port_c = motor_U.System_Order;
7706
7707 /* Inport: '<Root>/Encode_Sp' */
7708 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7709
7710 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7711 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7712 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7713 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7714 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7715 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7716 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7717 &motor_DWork.chu_jd);
7718
7719 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7720 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7721 tmp_0 = motor_B.Motor_HYDG.KP_e;
7722 if (tmp_0 < 2.147483648E+9) {
7723 if (tmp_0 >= -2.147483648E+9) {
7724 tmp = (int32_T)tmp_0;
7725 } else {
7726 tmp = MIN_int32_T;
7727 }
7728 } else {
7729 tmp = MAX_int32_T;
7730 }
7731
7732 motor_Y.JD_Error = tmp;
7733 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7734 if (tmp_0 < 2.147483648E+9) {
7735 if (tmp_0 >= -2.147483648E+9) {
7736 tmp = (int32_T)tmp_0;
7737 } else {
7738 tmp = MIN_int32_T;
7739 }
7740 } else {
7741 tmp = MAX_int32_T;
7742 }
7743
7744 motor_Y.JD_Out = tmp;
7745 } else if (motor_U.Up_Limit == 0) {
7746 /* Transition: '<S1>:1003' */
7747 /* 上限位开关低电平有效 */
7748 motor_DWork.is_HY_a = motor_IN_Int2_i;
7749 motor_DWork.temporalCounter_i1 = 0U;
7750
7751 /* Entry 'Int2': '<S1>:279' */
7752 motor_DWork.chu_jd += 0.002;
7753
7754 /* Inport: '<Root>/JD_In' */
7755 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7756 motor_B.JD_In_f = motor_U.JD_In;
7757 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7758
7759 /* Inport: '<Root>/System_Order' */
7760 motor_B.Slect_port_c = motor_U.System_Order;
7761
7762 /* Inport: '<Root>/Encode_Sp' */
7763 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7764
7765 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7766 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7767 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7768 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7769 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7770 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7771 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7772 &motor_DWork.chu_jd);
7773
7774 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7775 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7776 tmp_0 = motor_B.Motor_HYDG.KP_e;
7777 if (tmp_0 < 2.147483648E+9) {
7778 if (tmp_0 >= -2.147483648E+9) {
7779 tmp = (int32_T)tmp_0;
7780 } else {
7781 tmp = MIN_int32_T;
7782 }
7783 } else {
7784 tmp = MAX_int32_T;
7785 }
7786
7787 motor_Y.JD_Error = tmp;
7788 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7789 if (tmp_0 < 2.147483648E+9) {
7790 if (tmp_0 >= -2.147483648E+9) {
7791 tmp = (int32_T)tmp_0;
7792 } else {
7793 tmp = MIN_int32_T;
7794 }
7795 } else {
7796 tmp = MAX_int32_T;
7797 }
7798
7799 motor_Y.JD_Out = tmp;
7800 } else {
7801 motor_DWork.chu_jd += 0.01;
7802
7803 /* Inport: '<Root>/JD_In' */
7804 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
7805 motor_B.JD_In_f = motor_U.JD_In;
7806 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
7807
7808 /* Inport: '<Root>/System_Order' */
7809 motor_B.Slect_port_c = motor_U.System_Order;
7810
7811 /* Inport: '<Root>/Encode_Sp' */
7812 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
7813
7814 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
7815 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
7816 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
7817 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
7818 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
7819 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
7820 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7821 &motor_DWork.chu_jd);
7822
7823 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
7824 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
7825 tmp_0 = motor_B.Motor_HYDG.KP_e;
7826 if (tmp_0 < 2.147483648E+9) {
7827 if (tmp_0 >= -2.147483648E+9) {
7828 tmp = (int32_T)tmp_0;
7829 } else {
7830 tmp = MIN_int32_T;
7831 }
7832 } else {
7833 tmp = MAX_int32_T;
7834 }
7835
7836 motor_Y.JD_Error = tmp;
7837 tmp_0 = motor_B.Motor_HYDG.KP_JD;
7838 if (tmp_0 < 2.147483648E+9) {
7839 if (tmp_0 >= -2.147483648E+9) {
7840 tmp = (int32_T)tmp_0;
7841 } else {
7842 tmp = MIN_int32_T;
7843 }
7844 } else {
7845 tmp = MAX_int32_T;
7846 }
7847
7848 motor_Y.JD_Out = tmp;
7849 }
7850 break;
7851 }
7852 break;
7853
7854 case motor_IN_XHHY_m1:
7855 /* During 'XHHY': '<S1>:256' */
7856 switch (motor_DWork.is_XHHY_nl) {
7857 case motor_IN_Int:
7858 /* During 'Int': '<S1>:268' */
7859 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
7860 /* Transition: '<S1>:686' */
7861 motor_DWork.is_XHHY_nl = motor_IN_Int8;
7862 }
7863 break;
7864
7865 case motor_IN_Int1:
7866 /* Inport: '<Root>/Up_Limit' */
7867 /* During 'Int1': '<S1>:271' */
7868 if (motor_U.Up_Limit == 0) {
7869 /* Transition: '<S1>:687' */
7870 /* 上限位开关低电平有效 */
7871 motor_DWork.is_XHHY_nl = motor_IN_Int2_i;
7872 motor_DWork.temporalCounter_i1 = 0U;
7873
7874 /* Entry 'Int2': '<S1>:272' */
7875 motor_DWork.chu_jd += 0.002;
7876
7877 /* Inport: '<Root>/JD_In' */
7878 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
7879 motor_B.JD_In = motor_U.JD_In;
7880 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
7881
7882 /* Inport: '<Root>/Encode_Sp' */
7883 motor_B.Encode_Sp = motor_U.Encode_Sp;
7884
7885 /* Inport: '<Root>/System_Order' */
7886 motor_B.Slect_port = motor_U.System_Order;
7887
7888 /* Inport: '<Root>/SGWY_In' */
7889 motor_B.SGWY_In = motor_U.SGWY_In;
7890
7891 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
7892 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
7893 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
7894 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
7895 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
7896 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
7897 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
7898 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
7899 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7900 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
7901 &motor_DWork.chu_jd);
7902
7903 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
7904 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
7905 if (tmp_0 < 65536.0) {
7906 if (tmp_0 >= 0.0) {
7907 tmp_1 = (uint16_T)tmp_0;
7908 } else {
7909 tmp_1 = 0U;
7910 }
7911 } else {
7912 tmp_1 = MAX_uint16_T;
7913 }
7914
7915 motor_Y.PWMOUT = tmp_1;
7916 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
7917 if (tmp_0 < 2.147483648E+9) {
7918 if (tmp_0 >= -2.147483648E+9) {
7919 tmp = (int32_T)tmp_0;
7920 } else {
7921 tmp = MIN_int32_T;
7922 }
7923 } else {
7924 tmp = MAX_int32_T;
7925 }
7926
7927 motor_Y.JD_Out = tmp;
7928 tmp_0 = motor_B.Motor_XHHY.KP_e;
7929 if (tmp_0 < 2.147483648E+9) {
7930 if (tmp_0 >= -2.147483648E+9) {
7931 tmp = (int32_T)tmp_0;
7932 } else {
7933 tmp = MIN_int32_T;
7934 }
7935 } else {
7936 tmp = MAX_int32_T;
7937 }
7938
7939 motor_Y.JD_Error = tmp;
7940 } else if (motor_DWork.temporalCounter_i1 >= (uint32_T)(5.0 /
7941 motor_DWork.Ts)) {
7942 /* Transition: '<S1>:1038' */
7943 motor_Y.Flag_Up_GZ_limit = 0U;
7944
7945 /* 上限位开关故障$/ */
7946 motor_DWork.is_XHHY_nl = motor_IN_Int2_i;
7947 motor_DWork.temporalCounter_i1 = 0U;
7948
7949 /* Entry 'Int2': '<S1>:272' */
7950 motor_DWork.chu_jd += 0.002;
7951
7952 /* Inport: '<Root>/JD_In' */
7953 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
7954 motor_B.JD_In = motor_U.JD_In;
7955 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
7956
7957 /* Inport: '<Root>/Encode_Sp' */
7958 motor_B.Encode_Sp = motor_U.Encode_Sp;
7959
7960 /* Inport: '<Root>/System_Order' */
7961 motor_B.Slect_port = motor_U.System_Order;
7962
7963 /* Inport: '<Root>/SGWY_In' */
7964 motor_B.SGWY_In = motor_U.SGWY_In;
7965
7966 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
7967 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
7968 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
7969 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
7970 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
7971 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
7972 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
7973 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
7974 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
7975 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
7976 &motor_DWork.chu_jd);
7977
7978 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
7979 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
7980 if (tmp_0 < 65536.0) {
7981 if (tmp_0 >= 0.0) {
7982 tmp_1 = (uint16_T)tmp_0;
7983 } else {
7984 tmp_1 = 0U;
7985 }
7986 } else {
7987 tmp_1 = MAX_uint16_T;
7988 }
7989
7990 motor_Y.PWMOUT = tmp_1;
7991 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
7992 if (tmp_0 < 2.147483648E+9) {
7993 if (tmp_0 >= -2.147483648E+9) {
7994 tmp = (int32_T)tmp_0;
7995 } else {
7996 tmp = MIN_int32_T;
7997 }
7998 } else {
7999 tmp = MAX_int32_T;
8000 }
8001
8002 motor_Y.JD_Out = tmp;
8003 tmp_0 = motor_B.Motor_XHHY.KP_e;
8004 if (tmp_0 < 2.147483648E+9) {
8005 if (tmp_0 >= -2.147483648E+9) {
8006 tmp = (int32_T)tmp_0;
8007 } else {
8008 tmp = MIN_int32_T;
8009 }
8010 } else {
8011 tmp = MAX_int32_T;
8012 }
8013
8014 motor_Y.JD_Error = tmp;
8015 } else {
8016 motor_DWork.chu_jd += 0.002;
8017
8018 /* Inport: '<Root>/JD_In' */
8019 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
8020 motor_B.JD_In = motor_U.JD_In;
8021 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
8022
8023 /* Inport: '<Root>/Encode_Sp' */
8024 motor_B.Encode_Sp = motor_U.Encode_Sp;
8025
8026 /* Inport: '<Root>/System_Order' */
8027 motor_B.Slect_port = motor_U.System_Order;
8028
8029 /* Inport: '<Root>/SGWY_In' */
8030 motor_B.SGWY_In = motor_U.SGWY_In;
8031
8032 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
8033 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
8034 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
8035 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
8036 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
8037 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
8038 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
8039 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
8040 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8041 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
8042 &motor_DWork.chu_jd);
8043
8044 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
8045 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
8046 if (tmp_0 < 65536.0) {
8047 if (tmp_0 >= 0.0) {
8048 tmp_1 = (uint16_T)tmp_0;
8049 } else {
8050 tmp_1 = 0U;
8051 }
8052 } else {
8053 tmp_1 = MAX_uint16_T;
8054 }
8055
8056 motor_Y.PWMOUT = tmp_1;
8057 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
8058 if (tmp_0 < 2.147483648E+9) {
8059 if (tmp_0 >= -2.147483648E+9) {
8060 tmp = (int32_T)tmp_0;
8061 } else {
8062 tmp = MIN_int32_T;
8063 }
8064 } else {
8065 tmp = MAX_int32_T;
8066 }
8067
8068 motor_Y.JD_Out = tmp;
8069 tmp_0 = motor_B.Motor_XHHY.KP_e;
8070 if (tmp_0 < 2.147483648E+9) {
8071 if (tmp_0 >= -2.147483648E+9) {
8072 tmp = (int32_T)tmp_0;
8073 } else {
8074 tmp = MIN_int32_T;
8075 }
8076 } else {
8077 tmp = MAX_int32_T;
8078 }
8079
8080 motor_Y.JD_Error = tmp;
8081 }
8082 break;
8083
8084 case motor_IN_Int2_i:
8085 /* During 'Int2': '<S1>:272' */
8086 if (motor_DWork.temporalCounter_i1 >= 50U) {
8087 /* Transition: '<S1>:689' */
8088 motor_DWork.is_XHHY_nl = motor_IN_Int7;
8089 motor_DWork.temporalCounter_i1 = 0U;
8090 } else {
8091 motor_DWork.chu_jd += 0.002;
8092
8093 /* Inport: '<Root>/JD_In' */
8094 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
8095 motor_B.JD_In = motor_U.JD_In;
8096 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
8097
8098 /* Inport: '<Root>/Encode_Sp' */
8099 motor_B.Encode_Sp = motor_U.Encode_Sp;
8100
8101 /* Inport: '<Root>/System_Order' */
8102 motor_B.Slect_port = motor_U.System_Order;
8103
8104 /* Inport: '<Root>/SGWY_In' */
8105 motor_B.SGWY_In = motor_U.SGWY_In;
8106
8107 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
8108 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
8109 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
8110 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
8111 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
8112 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
8113 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
8114 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
8115 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8116 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
8117 &motor_DWork.chu_jd);
8118
8119 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
8120 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
8121 if (tmp_0 < 65536.0) {
8122 if (tmp_0 >= 0.0) {
8123 tmp_1 = (uint16_T)tmp_0;
8124 } else {
8125 tmp_1 = 0U;
8126 }
8127 } else {
8128 tmp_1 = MAX_uint16_T;
8129 }
8130
8131 motor_Y.PWMOUT = tmp_1;
8132 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
8133 if (tmp_0 < 2.147483648E+9) {
8134 if (tmp_0 >= -2.147483648E+9) {
8135 tmp = (int32_T)tmp_0;
8136 } else {
8137 tmp = MIN_int32_T;
8138 }
8139 } else {
8140 tmp = MAX_int32_T;
8141 }
8142
8143 motor_Y.JD_Out = tmp;
8144 tmp_0 = motor_B.Motor_XHHY.KP_e;
8145 if (tmp_0 < 2.147483648E+9) {
8146 if (tmp_0 >= -2.147483648E+9) {
8147 tmp = (int32_T)tmp_0;
8148 } else {
8149 tmp = MIN_int32_T;
8150 }
8151 } else {
8152 tmp = MAX_int32_T;
8153 }
8154
8155 motor_Y.JD_Error = tmp;
8156 }
8157 break;
8158
8159 case motor_IN_Int3_l:
8160 /* During 'Int3': '<S1>:267' */
8161 if (fabs(motor_DWork.chu_jd) < 0.1) {
8162 /* Transition: '<S1>:690' */
8163 motor_DWork.is_XHHY_nl = motor_IN_Int4_b;
8164 motor_DWork.temporalCounter_i1 = 0U;
8165
8166 /* Entry 'Int4': '<S1>:270' */
8167 motor_DWork.chu_jd = 0.0;
8168
8169 /* Inport: '<Root>/JD_In' */
8170 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
8171 motor_B.JD_In = motor_U.JD_In;
8172 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
8173
8174 /* Inport: '<Root>/Encode_Sp' */
8175 motor_B.Encode_Sp = motor_U.Encode_Sp;
8176
8177 /* Inport: '<Root>/System_Order' */
8178 motor_B.Slect_port = motor_U.System_Order;
8179
8180 /* Inport: '<Root>/SGWY_In' */
8181 motor_B.SGWY_In = motor_U.SGWY_In;
8182
8183 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
8184 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
8185 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
8186 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
8187 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
8188 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
8189 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
8190 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
8191 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8192 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
8193 &motor_DWork.chu_jd);
8194
8195 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
8196 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
8197 if (tmp_0 < 65536.0) {
8198 if (tmp_0 >= 0.0) {
8199 tmp_1 = (uint16_T)tmp_0;
8200 } else {
8201 tmp_1 = 0U;
8202 }
8203 } else {
8204 tmp_1 = MAX_uint16_T;
8205 }
8206
8207 motor_Y.PWMOUT = tmp_1;
8208 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
8209 if (tmp_0 < 2.147483648E+9) {
8210 if (tmp_0 >= -2.147483648E+9) {
8211 tmp = (int32_T)tmp_0;
8212 } else {
8213 tmp = MIN_int32_T;
8214 }
8215 } else {
8216 tmp = MAX_int32_T;
8217 }
8218
8219 motor_Y.JD_Out = tmp;
8220 tmp_0 = motor_B.Motor_XHHY.KP_e;
8221 if (tmp_0 < 2.147483648E+9) {
8222 if (tmp_0 >= -2.147483648E+9) {
8223 tmp = (int32_T)tmp_0;
8224 } else {
8225 tmp = MIN_int32_T;
8226 }
8227 } else {
8228 tmp = MAX_int32_T;
8229 }
8230
8231 motor_Y.JD_Error = tmp;
8232 } else {
8233 motor_DWork.chu_jd -= 0.01;
8234
8235 /* Inport: '<Root>/JD_In' */
8236 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
8237 motor_B.JD_In = motor_U.JD_In;
8238 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
8239
8240 /* Inport: '<Root>/Encode_Sp' */
8241 motor_B.Encode_Sp = motor_U.Encode_Sp;
8242
8243 /* Inport: '<Root>/System_Order' */
8244 motor_B.Slect_port = motor_U.System_Order;
8245
8246 /* Inport: '<Root>/SGWY_In' */
8247 motor_B.SGWY_In = motor_U.SGWY_In;
8248
8249 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
8250 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
8251 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
8252 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
8253 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
8254 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
8255 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
8256 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
8257 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8258 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
8259 &motor_DWork.chu_jd);
8260
8261 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
8262 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
8263 if (tmp_0 < 65536.0) {
8264 if (tmp_0 >= 0.0) {
8265 tmp_1 = (uint16_T)tmp_0;
8266 } else {
8267 tmp_1 = 0U;
8268 }
8269 } else {
8270 tmp_1 = MAX_uint16_T;
8271 }
8272
8273 motor_Y.PWMOUT = tmp_1;
8274 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
8275 if (tmp_0 < 2.147483648E+9) {
8276 if (tmp_0 >= -2.147483648E+9) {
8277 tmp = (int32_T)tmp_0;
8278 } else {
8279 tmp = MIN_int32_T;
8280 }
8281 } else {
8282 tmp = MAX_int32_T;
8283 }
8284
8285 motor_Y.JD_Out = tmp;
8286 tmp_0 = motor_B.Motor_XHHY.KP_e;
8287 if (tmp_0 < 2.147483648E+9) {
8288 if (tmp_0 >= -2.147483648E+9) {
8289 tmp = (int32_T)tmp_0;
8290 } else {
8291 tmp = MIN_int32_T;
8292 }
8293 } else {
8294 tmp = MAX_int32_T;
8295 }
8296
8297 motor_Y.JD_Error = tmp;
8298 }
8299 break;
8300
8301 case motor_IN_Int4_b:
8302 /* During 'Int4': '<S1>:270' */
8303 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
8304 /* Transition: '<S1>:692' */
8305 motor_DWork.is_XHHY_nl = motor_IN_Int5_a;
8306 motor_DWork.temporalCounter_i1 = 0U;
8307
8308 /* Entry 'Int5': '<S1>:269' */
8309 motor_Y.Motor_En = true;
8310
8311 /* 电机失能 */
8312 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
8313 } else {
8314 /* Inport: '<Root>/JD_In' */
8315 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
8316 motor_B.JD_In = motor_U.JD_In;
8317 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
8318
8319 /* Inport: '<Root>/Encode_Sp' */
8320 motor_B.Encode_Sp = motor_U.Encode_Sp;
8321
8322 /* Inport: '<Root>/System_Order' */
8323 motor_B.Slect_port = motor_U.System_Order;
8324
8325 /* Inport: '<Root>/SGWY_In' */
8326 motor_B.SGWY_In = motor_U.SGWY_In;
8327
8328 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
8329 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
8330 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
8331 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
8332 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
8333 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
8334 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
8335 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
8336 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8337 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
8338 &motor_DWork.chu_jd);
8339
8340 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
8341 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
8342 if (tmp_0 < 65536.0) {
8343 if (tmp_0 >= 0.0) {
8344 tmp_1 = (uint16_T)tmp_0;
8345 } else {
8346 tmp_1 = 0U;
8347 }
8348 } else {
8349 tmp_1 = MAX_uint16_T;
8350 }
8351
8352 motor_Y.PWMOUT = tmp_1;
8353 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
8354 if (tmp_0 < 2.147483648E+9) {
8355 if (tmp_0 >= -2.147483648E+9) {
8356 tmp = (int32_T)tmp_0;
8357 } else {
8358 tmp = MIN_int32_T;
8359 }
8360 } else {
8361 tmp = MAX_int32_T;
8362 }
8363
8364 motor_Y.JD_Out = tmp;
8365 tmp_0 = motor_B.Motor_XHHY.KP_e;
8366 if (tmp_0 < 2.147483648E+9) {
8367 if (tmp_0 >= -2.147483648E+9) {
8368 tmp = (int32_T)tmp_0;
8369 } else {
8370 tmp = MIN_int32_T;
8371 }
8372 } else {
8373 tmp = MAX_int32_T;
8374 }
8375
8376 motor_Y.JD_Error = tmp;
8377 }
8378 break;
8379
8380 case motor_IN_Int5_a:
8381 /* During 'Int5': '<S1>:269' */
8382 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
8383 /* Transition: '<S1>:691' */
8384 motor_DWork.is_XHHY_nl = motor_IN_Int6_m;
8385
8386 /* Entry 'Int6': '<S1>:266' */
8387 motor_Y.DCZD = true;
8388
8389 /* 开启制动 */
8390 }
8391 break;
8392
8393 case motor_IN_Int6_m:
8394 /* During 'Int6': '<S1>:266' */
8395 /* Transition: '<S1>:688' */
8396 /* Transition: '<S1>:663' */
8397 motor_DWork.is_XHHY_nl = motor_IN_NO_ACTIVE_CHILD;
8398 motor_DWork.is_Limit_Up_Test = motor_IN_NO_ACTIVE_CHILD;
8399 motor_DWork.is_Test_Mode = motor_IN_defult;
8400
8401 /* Entry 'defult': '<S1>:239' */
8402 motor_Y.Open_Result = 1U;
8403
8404 /* 开机状态位成功 */
8405 motor_DWork.In_State = 2U;
8406 break;
8407
8408 case motor_IN_Int7:
8409 /* During 'Int7': '<S1>:971' */
8410 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
8411 /* Transition: '<S1>:972' */
8412 motor_DWork.is_XHHY_nl = motor_IN_Int3_l;
8413
8414 /* Entry 'Int3': '<S1>:267' */
8415 motor_DWork.chu_jd -= 0.01;
8416
8417 /* Inport: '<Root>/JD_In' */
8418 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
8419 motor_B.JD_In = motor_U.JD_In;
8420 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
8421
8422 /* Inport: '<Root>/Encode_Sp' */
8423 motor_B.Encode_Sp = motor_U.Encode_Sp;
8424
8425 /* Inport: '<Root>/System_Order' */
8426 motor_B.Slect_port = motor_U.System_Order;
8427
8428 /* Inport: '<Root>/SGWY_In' */
8429 motor_B.SGWY_In = motor_U.SGWY_In;
8430
8431 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
8432 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
8433 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
8434 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
8435 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
8436 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
8437 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
8438 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
8439 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8440 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
8441 &motor_DWork.chu_jd);
8442
8443 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
8444 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
8445 if (tmp_0 < 65536.0) {
8446 if (tmp_0 >= 0.0) {
8447 tmp_1 = (uint16_T)tmp_0;
8448 } else {
8449 tmp_1 = 0U;
8450 }
8451 } else {
8452 tmp_1 = MAX_uint16_T;
8453 }
8454
8455 motor_Y.PWMOUT = tmp_1;
8456 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
8457 if (tmp_0 < 2.147483648E+9) {
8458 if (tmp_0 >= -2.147483648E+9) {
8459 tmp = (int32_T)tmp_0;
8460 } else {
8461 tmp = MIN_int32_T;
8462 }
8463 } else {
8464 tmp = MAX_int32_T;
8465 }
8466
8467 motor_Y.JD_Out = tmp;
8468 tmp_0 = motor_B.Motor_XHHY.KP_e;
8469 if (tmp_0 < 2.147483648E+9) {
8470 if (tmp_0 >= -2.147483648E+9) {
8471 tmp = (int32_T)tmp_0;
8472 } else {
8473 tmp = MIN_int32_T;
8474 }
8475 } else {
8476 tmp = MAX_int32_T;
8477 }
8478
8479 motor_Y.JD_Error = tmp;
8480 } else {
8481 /* Inport: '<Root>/JD_In' */
8482 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
8483 motor_B.JD_In = motor_U.JD_In;
8484 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
8485
8486 /* Inport: '<Root>/Encode_Sp' */
8487 motor_B.Encode_Sp = motor_U.Encode_Sp;
8488
8489 /* Inport: '<Root>/System_Order' */
8490 motor_B.Slect_port = motor_U.System_Order;
8491
8492 /* Inport: '<Root>/SGWY_In' */
8493 motor_B.SGWY_In = motor_U.SGWY_In;
8494
8495 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
8496 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
8497 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
8498 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
8499 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
8500 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
8501 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
8502 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
8503 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8504 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
8505 &motor_DWork.chu_jd);
8506
8507 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
8508 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
8509 if (tmp_0 < 65536.0) {
8510 if (tmp_0 >= 0.0) {
8511 tmp_1 = (uint16_T)tmp_0;
8512 } else {
8513 tmp_1 = 0U;
8514 }
8515 } else {
8516 tmp_1 = MAX_uint16_T;
8517 }
8518
8519 motor_Y.PWMOUT = tmp_1;
8520 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
8521 if (tmp_0 < 2.147483648E+9) {
8522 if (tmp_0 >= -2.147483648E+9) {
8523 tmp = (int32_T)tmp_0;
8524 } else {
8525 tmp = MIN_int32_T;
8526 }
8527 } else {
8528 tmp = MAX_int32_T;
8529 }
8530
8531 motor_Y.JD_Out = tmp;
8532 tmp_0 = motor_B.Motor_XHHY.KP_e;
8533 if (tmp_0 < 2.147483648E+9) {
8534 if (tmp_0 >= -2.147483648E+9) {
8535 tmp = (int32_T)tmp_0;
8536 } else {
8537 tmp = MIN_int32_T;
8538 }
8539 } else {
8540 tmp = MAX_int32_T;
8541 }
8542
8543 motor_Y.JD_Error = tmp;
8544 }
8545 break;
8546
8547 default:
8548 /* During 'Int8': '<S1>:988' */
8549 if (motor_DWork.chu_jd > 16.0) {
8550 /* Transition: '<S1>:989' */
8551 motor_DWork.is_XHHY_nl = motor_IN_Int1;
8552 motor_DWork.temporalCounter_i1 = 0U;
8553
8554 /* Entry 'Int1': '<S1>:271' */
8555 motor_DWork.chu_jd += 0.002;
8556
8557 /* Inport: '<Root>/JD_In' */
8558 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
8559 motor_B.JD_In = motor_U.JD_In;
8560 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
8561
8562 /* Inport: '<Root>/Encode_Sp' */
8563 motor_B.Encode_Sp = motor_U.Encode_Sp;
8564
8565 /* Inport: '<Root>/System_Order' */
8566 motor_B.Slect_port = motor_U.System_Order;
8567
8568 /* Inport: '<Root>/SGWY_In' */
8569 motor_B.SGWY_In = motor_U.SGWY_In;
8570
8571 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
8572 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
8573 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
8574 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
8575 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
8576 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
8577 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
8578 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
8579 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8580 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
8581 &motor_DWork.chu_jd);
8582
8583 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
8584 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
8585 if (tmp_0 < 65536.0) {
8586 if (tmp_0 >= 0.0) {
8587 tmp_1 = (uint16_T)tmp_0;
8588 } else {
8589 tmp_1 = 0U;
8590 }
8591 } else {
8592 tmp_1 = MAX_uint16_T;
8593 }
8594
8595 motor_Y.PWMOUT = tmp_1;
8596 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
8597 if (tmp_0 < 2.147483648E+9) {
8598 if (tmp_0 >= -2.147483648E+9) {
8599 tmp = (int32_T)tmp_0;
8600 } else {
8601 tmp = MIN_int32_T;
8602 }
8603 } else {
8604 tmp = MAX_int32_T;
8605 }
8606
8607 motor_Y.JD_Out = tmp;
8608 tmp_0 = motor_B.Motor_XHHY.KP_e;
8609 if (tmp_0 < 2.147483648E+9) {
8610 if (tmp_0 >= -2.147483648E+9) {
8611 tmp = (int32_T)tmp_0;
8612 } else {
8613 tmp = MIN_int32_T;
8614 }
8615 } else {
8616 tmp = MAX_int32_T;
8617 }
8618
8619 motor_Y.JD_Error = tmp;
8620 } else if (motor_U.Up_Limit == 0) {
8621 /* Transition: '<S1>:1004' */
8622 /* 上限位开关低电平有效 */
8623 motor_DWork.is_XHHY_nl = motor_IN_Int2_i;
8624 motor_DWork.temporalCounter_i1 = 0U;
8625
8626 /* Entry 'Int2': '<S1>:272' */
8627 motor_DWork.chu_jd += 0.002;
8628
8629 /* Inport: '<Root>/JD_In' */
8630 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
8631 motor_B.JD_In = motor_U.JD_In;
8632 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
8633
8634 /* Inport: '<Root>/Encode_Sp' */
8635 motor_B.Encode_Sp = motor_U.Encode_Sp;
8636
8637 /* Inport: '<Root>/System_Order' */
8638 motor_B.Slect_port = motor_U.System_Order;
8639
8640 /* Inport: '<Root>/SGWY_In' */
8641 motor_B.SGWY_In = motor_U.SGWY_In;
8642
8643 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
8644 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
8645 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
8646 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
8647 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
8648 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
8649 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
8650 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
8651 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8652 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
8653 &motor_DWork.chu_jd);
8654
8655 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
8656 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
8657 if (tmp_0 < 65536.0) {
8658 if (tmp_0 >= 0.0) {
8659 tmp_1 = (uint16_T)tmp_0;
8660 } else {
8661 tmp_1 = 0U;
8662 }
8663 } else {
8664 tmp_1 = MAX_uint16_T;
8665 }
8666
8667 motor_Y.PWMOUT = tmp_1;
8668 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
8669 if (tmp_0 < 2.147483648E+9) {
8670 if (tmp_0 >= -2.147483648E+9) {
8671 tmp = (int32_T)tmp_0;
8672 } else {
8673 tmp = MIN_int32_T;
8674 }
8675 } else {
8676 tmp = MAX_int32_T;
8677 }
8678
8679 motor_Y.JD_Out = tmp;
8680 tmp_0 = motor_B.Motor_XHHY.KP_e;
8681 if (tmp_0 < 2.147483648E+9) {
8682 if (tmp_0 >= -2.147483648E+9) {
8683 tmp = (int32_T)tmp_0;
8684 } else {
8685 tmp = MIN_int32_T;
8686 }
8687 } else {
8688 tmp = MAX_int32_T;
8689 }
8690
8691 motor_Y.JD_Error = tmp;
8692 } else {
8693 motor_DWork.chu_jd += 0.01;
8694
8695 /* Inport: '<Root>/JD_In' */
8696 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
8697 motor_B.JD_In = motor_U.JD_In;
8698 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
8699
8700 /* Inport: '<Root>/Encode_Sp' */
8701 motor_B.Encode_Sp = motor_U.Encode_Sp;
8702
8703 /* Inport: '<Root>/System_Order' */
8704 motor_B.Slect_port = motor_U.System_Order;
8705
8706 /* Inport: '<Root>/SGWY_In' */
8707 motor_B.SGWY_In = motor_U.SGWY_In;
8708
8709 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
8710 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
8711 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
8712 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
8713 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
8714 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
8715 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
8716 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
8717 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8718 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
8719 &motor_DWork.chu_jd);
8720
8721 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
8722 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
8723 if (tmp_0 < 65536.0) {
8724 if (tmp_0 >= 0.0) {
8725 tmp_1 = (uint16_T)tmp_0;
8726 } else {
8727 tmp_1 = 0U;
8728 }
8729 } else {
8730 tmp_1 = MAX_uint16_T;
8731 }
8732
8733 motor_Y.PWMOUT = tmp_1;
8734 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
8735 if (tmp_0 < 2.147483648E+9) {
8736 if (tmp_0 >= -2.147483648E+9) {
8737 tmp = (int32_T)tmp_0;
8738 } else {
8739 tmp = MIN_int32_T;
8740 }
8741 } else {
8742 tmp = MAX_int32_T;
8743 }
8744
8745 motor_Y.JD_Out = tmp;
8746 tmp_0 = motor_B.Motor_XHHY.KP_e;
8747 if (tmp_0 < 2.147483648E+9) {
8748 if (tmp_0 >= -2.147483648E+9) {
8749 tmp = (int32_T)tmp_0;
8750 } else {
8751 tmp = MIN_int32_T;
8752 }
8753 } else {
8754 tmp = MAX_int32_T;
8755 }
8756
8757 motor_Y.JD_Error = tmp;
8758 }
8759 break;
8760 }
8761 break;
8762
8763 default:
8764 motor_XHZY_a();
8765 break;
8766 }
8767}
8768
8769/* Function for Chart: '<Root>/Chart' */
8770static void motor_exit_internal_Test_Mode(void)
8771{
8772 /* Exit Internal 'Test_Mode': '<S1>:35' */
8773 motor_DWork.is_Test_Mode = motor_IN_NO_ACTIVE_CHILD;
8774
8775 /* Exit Internal 'Dspace_Test': '<S1>:1051' */
8776 motor_DWork.is_Dspace_Test = motor_IN_NO_ACTIVE_CHILD;
8777
8778 /* Exit Internal 'Elevation_Test': '<S1>:304' */
8779 motor_DWork.is_Elevation_Test = motor_IN_NO_ACTIVE_CHILD;
8780
8781 /* Exit Internal 'Limit_Down_Test': '<S1>:240' */
8782 /* Exit Internal 'HY': '<S1>:280' */
8783 motor_DWork.is_HY_h = motor_IN_NO_ACTIVE_CHILD;
8784 motor_DWork.is_Limit_Down_Test = motor_IN_NO_ACTIVE_CHILD;
8785
8786 /* Exit Internal 'XHHY': '<S1>:296' */
8787 motor_DWork.is_XHHY_f = motor_IN_NO_ACTIVE_CHILD;
8788
8789 /* Exit Internal 'XHZY': '<S1>:288' */
8790 motor_DWork.is_XHZY_i = motor_IN_NO_ACTIVE_CHILD;
8791
8792 /* Exit Internal 'Limit_Up_Test': '<S1>:238' */
8793 /* Exit Internal 'HY': '<S1>:258' */
8794 motor_DWork.is_HY_a = motor_IN_NO_ACTIVE_CHILD;
8795 motor_DWork.is_Limit_Up_Test = motor_IN_NO_ACTIVE_CHILD;
8796
8797 /* Exit Internal 'XHHY': '<S1>:256' */
8798 motor_DWork.is_XHHY_nl = motor_IN_NO_ACTIVE_CHILD;
8799
8800 /* Exit Internal 'XHZY': '<S1>:257' */
8801 motor_DWork.is_XHZY_a = motor_IN_NO_ACTIVE_CHILD;
8802}
8803
8804/* Function for Chart: '<Root>/Chart' */
8805static uint8_T motor_Init_Result_Check(uint8_T In1, uint8_T In2, real_T In3)
8806{
8807 uint8_T y;
8808
8809 /* MATLAB Function 'Init_Result_Check': '<S1>:105' */
8810 if ((In1 == 1) && (In2 == 1) && (In3 == 1.0)) {
8811 /* '<S1>:105:2' */
8812 /* '<S1>:105:3' */
8813 y = 1U;
8814 } else {
8815 /* '<S1>:105:5' */
8816 y = 0U;
8817 }
8818
8819 return y;
8820}
8821
8822/* Function for Chart: '<Root>/Chart' */
8823static void motor_hy(void)
8824{
8825 boolean_T guard1 = false;
8826 real_T tmp;
8827 int32_T saturatedUnaryMinus;
8828
8829 /* During 'hy': '<S1>:62' */
8830 switch (motor_DWork.is_hy) {
8831 case motor_IN_P1:
8832 /* During 'P1': '<S1>:39' */
8833 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(2.0 / motor_DWork.Ts)) {
8834 /* Transition: '<S1>:452' */
8835 /* 等待2S */
8836 if (motor_DWork.Encode_Pos0 <= 293092.0) {
8837 /* Transition: '<S1>:453' */
8838 /* 293092为2.5度 */
8839 motor_DWork.is_hy = motor_IN_P8;
8840
8841 /* Entry 'P8': '<S1>:29' */
8842 motor_Y.Flag_Motor_Error = 0U;
8843
8844 /* 电机故障,不能正转
8845 Motor_En=1;
8846 电机失能,关 */
8847 } else {
8848 if (motor_U.Encode_Sp < 0) {
8849 /* Inport: '<Root>/Encode_Sp' */
8850 saturatedUnaryMinus = motor_U.Encode_Sp;
8851 if (saturatedUnaryMinus <= MIN_int32_T) {
8852 saturatedUnaryMinus = MAX_int32_T;
8853 } else {
8854 saturatedUnaryMinus = -saturatedUnaryMinus;
8855 }
8856 } else {
8857 /* Inport: '<Root>/Encode_Sp' */
8858 saturatedUnaryMinus = motor_U.Encode_Sp;
8859 }
8860
8861 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Sp_Min) {
8862 /* Transition: '<S1>:454' */
8863 /* 判断电机是否到达上机械限位
8864 故障(卡死)
8865 编码器故障 */
8866 motor_DWork.is_hy = motor_IN_P3;
8867
8868 /* Entry 'P3': '<S1>:26' */
8869 /* en:PWMOUT=PWM_Value_Down;
8870 du:PWMOUT=PWM_Value_Down;%电机下行$/
8871 on after(4/Ts,tick):Flag_Up_GZ_limit= uint8(0);%上限位开关故障$/
8872 Motor_En=1;
8873 电机失能,关
8874 PWMOUT=PWM_Value_Mid;
8875 4S之后停机$/ */
8876 motor_Y.Flag_Up_GZ_limit = 0U;
8877
8878 /* 上限位开关故障$/ */
8879 } else {
8880 guard1 = true;
8881 }
8882 }
8883 } else {
8884 guard1 = true;
8885 }
8886 break;
8887
8888 case motor_IN_P10:
8889 /* During 'P10': '<S1>:68' */
8890 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.2 / motor_DWork.Ts)) {
8891 /* Transition: '<S1>:461' */
8892 motor_DWork.is_hy = motor_IN_p11;
8893
8894 /* Entry 'p11': '<S1>:40' */
8895 motor_DWork.Encode_Pos0 = motor_DWork.Encode_Pos3;
8896 motor_DWork.Average_En = 1.0;
8897 motor_DWork.chu_jd = -20.5;
8898 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
8899 } else {
8900 motor_DWork.chu_jd -= 0.003;
8901
8902 /* Inport: '<Root>/JD_In' */
8903 /* 电机下行 */
8904 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
8905 motor_B.JD_In_f = motor_U.JD_In;
8906 tmp = motor_DWork.Encode_Pos0;
8907 if (tmp < 2.147483648E+9) {
8908 if (tmp >= -2.147483648E+9) {
8909 saturatedUnaryMinus = (int32_T)tmp;
8910 } else {
8911 saturatedUnaryMinus = MIN_int32_T;
8912 }
8913 } else {
8914 saturatedUnaryMinus = MAX_int32_T;
8915 }
8916
8917 motor_B.Encode_Pos_i = saturatedUnaryMinus;
8918
8919 /* Inport: '<Root>/System_Order' */
8920 motor_B.Slect_port_c = motor_U.System_Order;
8921
8922 /* Inport: '<Root>/Encode_Sp' */
8923 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
8924
8925 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
8926 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
8927 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
8928 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
8929 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
8930 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
8931 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
8932 &motor_DWork.chu_jd);
8933
8934 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
8935 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
8936 tmp = motor_B.Motor_HYDG.KP_e;
8937 if (tmp < 2.147483648E+9) {
8938 if (tmp >= -2.147483648E+9) {
8939 saturatedUnaryMinus = (int32_T)tmp;
8940 } else {
8941 saturatedUnaryMinus = MIN_int32_T;
8942 }
8943 } else {
8944 saturatedUnaryMinus = MAX_int32_T;
8945 }
8946
8947 motor_Y.JD_Error = saturatedUnaryMinus;
8948 tmp = motor_B.Motor_HYDG.KP_JD;
8949 if (tmp < 2.147483648E+9) {
8950 if (tmp >= -2.147483648E+9) {
8951 saturatedUnaryMinus = (int32_T)tmp;
8952 } else {
8953 saturatedUnaryMinus = MIN_int32_T;
8954 }
8955 } else {
8956 saturatedUnaryMinus = MAX_int32_T;
8957 }
8958
8959 motor_Y.JD_Out = saturatedUnaryMinus;
8960
8961 /* 电机触碰到机械限位 */
8962 }
8963 break;
8964
8965 case motor_IN_P11:
8966 /* During 'P11': '<S1>:1175' */
8967 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
8968 /* Transition: '<S1>:1169' */
8969 motor_DWork.is_hy = motor_IN_P2;
8970 motor_DWork.temporalCounter_i1 = 0U;
8971
8972 /* Entry 'P2': '<S1>:41' */
8973 motor_DWork.chu_jd -= 0.01;
8974
8975 /* Inport: '<Root>/JD_In' */
8976 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
8977 motor_B.JD_In_f = motor_U.JD_In;
8978 tmp = motor_DWork.Encode_Pos0;
8979 if (tmp < 2.147483648E+9) {
8980 if (tmp >= -2.147483648E+9) {
8981 saturatedUnaryMinus = (int32_T)tmp;
8982 } else {
8983 saturatedUnaryMinus = MIN_int32_T;
8984 }
8985 } else {
8986 saturatedUnaryMinus = MAX_int32_T;
8987 }
8988
8989 motor_B.Encode_Pos_i = saturatedUnaryMinus;
8990
8991 /* Inport: '<Root>/System_Order' */
8992 motor_B.Slect_port_c = motor_U.System_Order;
8993
8994 /* Inport: '<Root>/Encode_Sp' */
8995 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
8996
8997 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
8998 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
8999 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
9000 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
9001 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
9002 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
9003 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9004 &motor_DWork.chu_jd);
9005
9006 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
9007 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
9008 tmp = motor_B.Motor_HYDG.KP_e;
9009 if (tmp < 2.147483648E+9) {
9010 if (tmp >= -2.147483648E+9) {
9011 saturatedUnaryMinus = (int32_T)tmp;
9012 } else {
9013 saturatedUnaryMinus = MIN_int32_T;
9014 }
9015 } else {
9016 saturatedUnaryMinus = MAX_int32_T;
9017 }
9018
9019 motor_Y.JD_Error = saturatedUnaryMinus;
9020 tmp = motor_B.Motor_HYDG.KP_JD;
9021 if (tmp < 2.147483648E+9) {
9022 if (tmp >= -2.147483648E+9) {
9023 saturatedUnaryMinus = (int32_T)tmp;
9024 } else {
9025 saturatedUnaryMinus = MIN_int32_T;
9026 }
9027 } else {
9028 saturatedUnaryMinus = MAX_int32_T;
9029 }
9030
9031 motor_Y.JD_Out = saturatedUnaryMinus;
9032 }
9033 break;
9034
9035 case motor_IN_P2:
9036 /* Inport: '<Root>/Encode_Sp' */
9037 /* Inport: '<Root>/Down_Limit' */
9038 /* During 'P2': '<S1>:41' */
9039 if (motor_U.Encode_Sp < 0) {
9040 saturatedUnaryMinus = motor_U.Encode_Sp;
9041 if (saturatedUnaryMinus <= MIN_int32_T) {
9042 saturatedUnaryMinus = MAX_int32_T;
9043 } else {
9044 saturatedUnaryMinus = -saturatedUnaryMinus;
9045 }
9046 } else {
9047 saturatedUnaryMinus = motor_U.Encode_Sp;
9048 }
9049
9050 if ((motor_DWork.temporalCounter_i1 >= (uint32_T)(3.0 / motor_DWork.Ts)) &&
9051 ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Sp_Min)) {
9052 /* Transition: '<S1>:458' */
9053 /* 等待3S */
9054 /* Transition: '<S1>:456' */
9055 /* 判断电机是否到达下机械限位
9056 故障(卡死) */
9057 motor_DWork.is_hy = motor_IN_P5;
9058
9059 /* Entry 'P5': '<S1>:24' */
9060 /* en:PWMOUT=PWM_Value_Up;
9061 du:PWMOUT=PWM_Value_Up;%电机上行
9062 on after(4/Ts,tick):Flag_Down_GZ_limit=0;%下限位开关故障
9063 Motor_En=1;
9064 PWMOUT=PWM_Value_Mid;
9065 4S之后停机 */
9066 motor_Y.Flag_Down_GZ_limit = 0U;
9067
9068 /* 下限位开关故障 */
9069 } else if (motor_U.Down_Limit == 0) {
9070 /* Transition: '<S1>:459' */
9071 /* 下限位开关低电平有效 */
9072 motor_DWork.is_hy = motor_IN_P4;
9073
9074 /* Entry 'P4': '<S1>:28' */
9075 motor_DWork.chu_jd -= 0.003;
9076
9077 /* Inport: '<Root>/JD_In' */
9078 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
9079 motor_B.JD_In_f = motor_U.JD_In;
9080 tmp = motor_DWork.Encode_Pos0;
9081 if (tmp < 2.147483648E+9) {
9082 if (tmp >= -2.147483648E+9) {
9083 saturatedUnaryMinus = (int32_T)tmp;
9084 } else {
9085 saturatedUnaryMinus = MIN_int32_T;
9086 }
9087 } else {
9088 saturatedUnaryMinus = MAX_int32_T;
9089 }
9090
9091 motor_B.Encode_Pos_i = saturatedUnaryMinus;
9092
9093 /* Inport: '<Root>/System_Order' */
9094 motor_B.Slect_port_c = motor_U.System_Order;
9095
9096 /* Inport: '<Root>/Encode_Sp' */
9097 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
9098
9099 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
9100 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
9101 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
9102 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
9103 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
9104 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
9105 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9106 &motor_DWork.chu_jd);
9107
9108 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
9109 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
9110 tmp = motor_B.Motor_HYDG.KP_e;
9111 if (tmp < 2.147483648E+9) {
9112 if (tmp >= -2.147483648E+9) {
9113 saturatedUnaryMinus = (int32_T)tmp;
9114 } else {
9115 saturatedUnaryMinus = MIN_int32_T;
9116 }
9117 } else {
9118 saturatedUnaryMinus = MAX_int32_T;
9119 }
9120
9121 motor_Y.JD_Error = saturatedUnaryMinus;
9122 tmp = motor_B.Motor_HYDG.KP_JD;
9123 if (tmp < 2.147483648E+9) {
9124 if (tmp >= -2.147483648E+9) {
9125 saturatedUnaryMinus = (int32_T)tmp;
9126 } else {
9127 saturatedUnaryMinus = MIN_int32_T;
9128 }
9129 } else {
9130 saturatedUnaryMinus = MAX_int32_T;
9131 }
9132
9133 motor_Y.JD_Out = saturatedUnaryMinus;
9134 } else {
9135 motor_DWork.chu_jd -= 0.01;
9136
9137 /* Inport: '<Root>/JD_In' */
9138 /* 电机下行 */
9139 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
9140 motor_B.JD_In_f = motor_U.JD_In;
9141 tmp = motor_DWork.Encode_Pos0;
9142 if (tmp < 2.147483648E+9) {
9143 if (tmp >= -2.147483648E+9) {
9144 saturatedUnaryMinus = (int32_T)tmp;
9145 } else {
9146 saturatedUnaryMinus = MIN_int32_T;
9147 }
9148 } else {
9149 saturatedUnaryMinus = MAX_int32_T;
9150 }
9151
9152 motor_B.Encode_Pos_i = saturatedUnaryMinus;
9153
9154 /* Inport: '<Root>/System_Order' */
9155 motor_B.Slect_port_c = motor_U.System_Order;
9156
9157 /* Inport: '<Root>/Encode_Sp' */
9158 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
9159
9160 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
9161 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
9162 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
9163 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
9164 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
9165 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
9166 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9167 &motor_DWork.chu_jd);
9168
9169 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
9170 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
9171 tmp = motor_B.Motor_HYDG.KP_e;
9172 if (tmp < 2.147483648E+9) {
9173 if (tmp >= -2.147483648E+9) {
9174 saturatedUnaryMinus = (int32_T)tmp;
9175 } else {
9176 saturatedUnaryMinus = MIN_int32_T;
9177 }
9178 } else {
9179 saturatedUnaryMinus = MAX_int32_T;
9180 }
9181
9182 motor_Y.JD_Error = saturatedUnaryMinus;
9183 tmp = motor_B.Motor_HYDG.KP_JD;
9184 if (tmp < 2.147483648E+9) {
9185 if (tmp >= -2.147483648E+9) {
9186 saturatedUnaryMinus = (int32_T)tmp;
9187 } else {
9188 saturatedUnaryMinus = MIN_int32_T;
9189 }
9190 } else {
9191 saturatedUnaryMinus = MAX_int32_T;
9192 }
9193
9194 motor_Y.JD_Out = saturatedUnaryMinus;
9195 }
9196
9197 /* End of Inport: '<Root>/Down_Limit' */
9198 break;
9199
9200 case motor_IN_P3:
9201 /* During 'P3': '<S1>:26' */
9202 /* Transition: '<S1>:1030' */
9203 motor_DWork.is_hy = motor_IN_P2;
9204 motor_DWork.temporalCounter_i1 = 0U;
9205
9206 /* Entry 'P2': '<S1>:41' */
9207 motor_DWork.chu_jd -= 0.01;
9208
9209 /* Inport: '<Root>/JD_In' */
9210 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
9211 motor_B.JD_In_f = motor_U.JD_In;
9212 tmp = motor_DWork.Encode_Pos0;
9213 if (tmp < 2.147483648E+9) {
9214 if (tmp >= -2.147483648E+9) {
9215 saturatedUnaryMinus = (int32_T)tmp;
9216 } else {
9217 saturatedUnaryMinus = MIN_int32_T;
9218 }
9219 } else {
9220 saturatedUnaryMinus = MAX_int32_T;
9221 }
9222
9223 motor_B.Encode_Pos_i = saturatedUnaryMinus;
9224
9225 /* Inport: '<Root>/System_Order' */
9226 motor_B.Slect_port_c = motor_U.System_Order;
9227
9228 /* Inport: '<Root>/Encode_Sp' */
9229 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
9230
9231 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
9232 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
9233 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
9234 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
9235 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
9236 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
9237 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9238 &motor_DWork.chu_jd);
9239
9240 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
9241 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
9242 tmp = motor_B.Motor_HYDG.KP_e;
9243 if (tmp < 2.147483648E+9) {
9244 if (tmp >= -2.147483648E+9) {
9245 saturatedUnaryMinus = (int32_T)tmp;
9246 } else {
9247 saturatedUnaryMinus = MIN_int32_T;
9248 }
9249 } else {
9250 saturatedUnaryMinus = MAX_int32_T;
9251 }
9252
9253 motor_Y.JD_Error = saturatedUnaryMinus;
9254 tmp = motor_B.Motor_HYDG.KP_JD;
9255 if (tmp < 2.147483648E+9) {
9256 if (tmp >= -2.147483648E+9) {
9257 saturatedUnaryMinus = (int32_T)tmp;
9258 } else {
9259 saturatedUnaryMinus = MIN_int32_T;
9260 }
9261 } else {
9262 saturatedUnaryMinus = MAX_int32_T;
9263 }
9264
9265 motor_Y.JD_Out = saturatedUnaryMinus;
9266 break;
9267
9268 case motor_IN_P4:
9269 /* Inport: '<Root>/Encode_Sp' */
9270 /* During 'P4': '<S1>:28' */
9271 if (motor_U.Encode_Sp < 0) {
9272 saturatedUnaryMinus = motor_U.Encode_Sp;
9273 if (saturatedUnaryMinus <= MIN_int32_T) {
9274 saturatedUnaryMinus = MAX_int32_T;
9275 } else {
9276 saturatedUnaryMinus = -saturatedUnaryMinus;
9277 }
9278 } else {
9279 saturatedUnaryMinus = motor_U.Encode_Sp;
9280 }
9281
9282 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Sp_Min) {
9283 /* Transition: '<S1>:460' */
9284 /* 判断电机是否到达下机械限位 */
9285 motor_DWork.is_hy = motor_IN_P10;
9286 motor_DWork.temporalCounter_i1 = 0U;
9287
9288 /* Entry 'P10': '<S1>:68' */
9289 motor_DWork.chu_jd -= 0.003;
9290
9291 /* Inport: '<Root>/JD_In' */
9292 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
9293 motor_B.JD_In_f = motor_U.JD_In;
9294 tmp = motor_DWork.Encode_Pos0;
9295 if (tmp < 2.147483648E+9) {
9296 if (tmp >= -2.147483648E+9) {
9297 saturatedUnaryMinus = (int32_T)tmp;
9298 } else {
9299 saturatedUnaryMinus = MIN_int32_T;
9300 }
9301 } else {
9302 saturatedUnaryMinus = MAX_int32_T;
9303 }
9304
9305 motor_B.Encode_Pos_i = saturatedUnaryMinus;
9306
9307 /* Inport: '<Root>/System_Order' */
9308 motor_B.Slect_port_c = motor_U.System_Order;
9309
9310 /* Inport: '<Root>/Encode_Sp' */
9311 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
9312
9313 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
9314 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
9315 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
9316 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
9317 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
9318 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
9319 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9320 &motor_DWork.chu_jd);
9321
9322 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
9323 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
9324 tmp = motor_B.Motor_HYDG.KP_e;
9325 if (tmp < 2.147483648E+9) {
9326 if (tmp >= -2.147483648E+9) {
9327 saturatedUnaryMinus = (int32_T)tmp;
9328 } else {
9329 saturatedUnaryMinus = MIN_int32_T;
9330 }
9331 } else {
9332 saturatedUnaryMinus = MAX_int32_T;
9333 }
9334
9335 motor_Y.JD_Error = saturatedUnaryMinus;
9336 tmp = motor_B.Motor_HYDG.KP_JD;
9337 if (tmp < 2.147483648E+9) {
9338 if (tmp >= -2.147483648E+9) {
9339 saturatedUnaryMinus = (int32_T)tmp;
9340 } else {
9341 saturatedUnaryMinus = MIN_int32_T;
9342 }
9343 } else {
9344 saturatedUnaryMinus = MAX_int32_T;
9345 }
9346
9347 motor_Y.JD_Out = saturatedUnaryMinus;
9348 } else {
9349 motor_DWork.chu_jd -= 0.003;
9350
9351 /* Inport: '<Root>/JD_In' */
9352 /* 电机下行 */
9353 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
9354 motor_B.JD_In_f = motor_U.JD_In;
9355 tmp = motor_DWork.Encode_Pos0;
9356 if (tmp < 2.147483648E+9) {
9357 if (tmp >= -2.147483648E+9) {
9358 saturatedUnaryMinus = (int32_T)tmp;
9359 } else {
9360 saturatedUnaryMinus = MIN_int32_T;
9361 }
9362 } else {
9363 saturatedUnaryMinus = MAX_int32_T;
9364 }
9365
9366 motor_B.Encode_Pos_i = saturatedUnaryMinus;
9367
9368 /* Inport: '<Root>/System_Order' */
9369 motor_B.Slect_port_c = motor_U.System_Order;
9370
9371 /* Inport: '<Root>/Encode_Sp' */
9372 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
9373
9374 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
9375 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
9376 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
9377 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
9378 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
9379 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
9380 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9381 &motor_DWork.chu_jd);
9382
9383 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
9384 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
9385 tmp = motor_B.Motor_HYDG.KP_e;
9386 if (tmp < 2.147483648E+9) {
9387 if (tmp >= -2.147483648E+9) {
9388 saturatedUnaryMinus = (int32_T)tmp;
9389 } else {
9390 saturatedUnaryMinus = MIN_int32_T;
9391 }
9392 } else {
9393 saturatedUnaryMinus = MAX_int32_T;
9394 }
9395
9396 motor_Y.JD_Error = saturatedUnaryMinus;
9397 tmp = motor_B.Motor_HYDG.KP_JD;
9398 if (tmp < 2.147483648E+9) {
9399 if (tmp >= -2.147483648E+9) {
9400 saturatedUnaryMinus = (int32_T)tmp;
9401 } else {
9402 saturatedUnaryMinus = MIN_int32_T;
9403 }
9404 } else {
9405 saturatedUnaryMinus = MAX_int32_T;
9406 }
9407
9408 motor_Y.JD_Out = saturatedUnaryMinus;
9409
9410 /* 电机触碰到机械限位 */
9411 }
9412 break;
9413
9414 case motor_IN_P5:
9415 /* During 'P5': '<S1>:24' */
9416 /* Transition: '<S1>:1029' */
9417 motor_DWork.is_hy = motor_IN_p11;
9418
9419 /* Entry 'p11': '<S1>:40' */
9420 motor_DWork.Encode_Pos0 = motor_DWork.Encode_Pos3;
9421 motor_DWork.Average_En = 1.0;
9422 motor_DWork.chu_jd = -20.5;
9423 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
9424 break;
9425
9426 case motor_IN_P8:
9427 /* During 'P8': '<S1>:29' */
9428 break;
9429
9430 case motor_IN_p11:
9431 /* During 'p11': '<S1>:40' */
9432 if (fabs(motor_DWork.chu_jd) <= 0.02) {
9433 /* Transition: '<S1>:463' */
9434 motor_DWork.is_hy = motor_IN_p12;
9435 motor_DWork.temporalCounter_i1 = 0U;
9436
9437 /* Entry 'p12': '<S1>:125' */
9438 motor_Y.Motor_En = true;
9439
9440 /* 关电机 */
9441 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
9442 } else {
9443 motor_DWork.chu_jd += 0.01;
9444
9445 /* Inport: '<Root>/JD_In' */
9446 /* 电机上行回零点 */
9447 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
9448 motor_B.JD_In_f = motor_U.JD_In;
9449 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
9450
9451 /* Inport: '<Root>/System_Order' */
9452 motor_B.Slect_port_c = motor_U.System_Order;
9453
9454 /* Inport: '<Root>/Encode_Sp' */
9455 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
9456
9457 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
9458 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
9459 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
9460 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
9461 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
9462 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
9463 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9464 &motor_DWork.chu_jd);
9465
9466 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
9467 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
9468 tmp = motor_B.Motor_HYDG.KP_e;
9469 if (tmp < 2.147483648E+9) {
9470 if (tmp >= -2.147483648E+9) {
9471 saturatedUnaryMinus = (int32_T)tmp;
9472 } else {
9473 saturatedUnaryMinus = MIN_int32_T;
9474 }
9475 } else {
9476 saturatedUnaryMinus = MAX_int32_T;
9477 }
9478
9479 motor_Y.JD_Error = saturatedUnaryMinus;
9480 tmp = motor_B.Motor_HYDG.KP_JD;
9481 if (tmp < 2.147483648E+9) {
9482 if (tmp >= -2.147483648E+9) {
9483 saturatedUnaryMinus = (int32_T)tmp;
9484 } else {
9485 saturatedUnaryMinus = MIN_int32_T;
9486 }
9487 } else {
9488 saturatedUnaryMinus = MAX_int32_T;
9489 }
9490
9491 motor_Y.JD_Out = saturatedUnaryMinus;
9492 }
9493 break;
9494
9495 default:
9496 /* During 'p12': '<S1>:125' */
9497 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.2 / motor_DWork.Ts)) {
9498 /* Transition: '<S1>:462' */
9499 /* Transition: '<S1>:406' */
9500 motor_DWork.is_hy = motor_IN_NO_ACTIVE_CHILD;
9501 motor_DWork.is_Initialize = motor_IN_Check_Reset;
9502
9503 /* Entry 'Check_Reset': '<S1>:71' */
9504 motor_Y.Ini_Result = motor_Init_Result_Check(motor_Y.Flag_Cur,
9505 motor_Y.Flag_Temp_Up, (real_T)motor_Y.Flag_Temp_Down);
9506 motor_Y.DCZD = true;
9507 }
9508 break;
9509 }
9510
9511 if (guard1) {
9512 /* Inport: '<Root>/Up_Limit' */
9513 if (motor_U.Up_Limit == 0) {
9514 /* Transition: '<S1>:455' */
9515 /* 上限位开关低电平有效 */
9516 motor_DWork.is_hy = motor_IN_P11;
9517 motor_DWork.temporalCounter_i1 = 0U;
9518
9519 /* Entry 'P11': '<S1>:1175' */
9520 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
9521 } else {
9522 motor_DWork.chu_jd += 0.01;
9523
9524 /* Inport: '<Root>/JD_In' */
9525 /* 电机上行 */
9526 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
9527 motor_B.JD_In_f = motor_U.JD_In;
9528 tmp = motor_DWork.Encode_Pos0;
9529 if (tmp < 2.147483648E+9) {
9530 if (tmp >= -2.147483648E+9) {
9531 saturatedUnaryMinus = (int32_T)tmp;
9532 } else {
9533 saturatedUnaryMinus = MIN_int32_T;
9534 }
9535 } else {
9536 saturatedUnaryMinus = MAX_int32_T;
9537 }
9538
9539 motor_B.Encode_Pos_i = saturatedUnaryMinus;
9540
9541 /* Inport: '<Root>/System_Order' */
9542 motor_B.Slect_port_c = motor_U.System_Order;
9543
9544 /* Inport: '<Root>/Encode_Sp' */
9545 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
9546
9547 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
9548 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
9549 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
9550 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
9551 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
9552 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
9553 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9554 &motor_DWork.chu_jd);
9555
9556 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
9557 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
9558 tmp = motor_B.Motor_HYDG.KP_e;
9559 if (tmp < 2.147483648E+9) {
9560 if (tmp >= -2.147483648E+9) {
9561 saturatedUnaryMinus = (int32_T)tmp;
9562 } else {
9563 saturatedUnaryMinus = MIN_int32_T;
9564 }
9565 } else {
9566 saturatedUnaryMinus = MAX_int32_T;
9567 }
9568
9569 motor_Y.JD_Error = saturatedUnaryMinus;
9570 tmp = motor_B.Motor_HYDG.KP_JD;
9571 if (tmp < 2.147483648E+9) {
9572 if (tmp >= -2.147483648E+9) {
9573 saturatedUnaryMinus = (int32_T)tmp;
9574 } else {
9575 saturatedUnaryMinus = MIN_int32_T;
9576 }
9577 } else {
9578 saturatedUnaryMinus = MAX_int32_T;
9579 }
9580
9581 motor_Y.JD_Out = saturatedUnaryMinus;
9582 }
9583
9584 /* End of Inport: '<Root>/Up_Limit' */
9585 }
9586}
9587
9588/* Function for Chart: '<Root>/Chart' */
9589static void motor_xhhy(void)
9590{
9591 boolean_T guard1 = false;
9592 uint16_T tmp;
9593 real_T tmp_0;
9594 int32_T saturatedUnaryMinus;
9595
9596 /* During 'xhhy': '<S1>:65' */
9597 switch (motor_DWork.is_xhhy) {
9598 case motor_IN_P1:
9599 /* Inport: '<Root>/Up_Limit' incorporates:
9600 * Inport: '<Root>/Encode_Sp'
9601 */
9602 /* During 'P1': '<S1>:25' */
9603 if (motor_U.Up_Limit == 0) {
9604 /* Transition: '<S1>:468' */
9605 /* 上限位开关低电平有效 */
9606 motor_DWork.is_xhhy = motor_IN_P10;
9607
9608 /* Entry 'P10': '<S1>:30' */
9609 motor_DWork.chu_jd += 0.003;
9610
9611 /* Inport: '<Root>/JD_In' */
9612 /* 电机上行 */
9613 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
9614 motor_B.JD_In = motor_U.JD_In;
9615 tmp_0 = motor_DWork.Encode_Pos0;
9616 if (tmp_0 < 2.147483648E+9) {
9617 if (tmp_0 >= -2.147483648E+9) {
9618 saturatedUnaryMinus = (int32_T)tmp_0;
9619 } else {
9620 saturatedUnaryMinus = MIN_int32_T;
9621 }
9622 } else {
9623 saturatedUnaryMinus = MAX_int32_T;
9624 }
9625
9626 motor_B.Encode_Pos_c = saturatedUnaryMinus;
9627
9628 /* Inport: '<Root>/Encode_Sp' */
9629 motor_B.Encode_Sp = motor_U.Encode_Sp;
9630
9631 /* Inport: '<Root>/System_Order' */
9632 motor_B.Slect_port = motor_U.System_Order;
9633
9634 /* Inport: '<Root>/SGWY_In' */
9635 motor_B.SGWY_In = motor_U.SGWY_In;
9636
9637 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
9638 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
9639 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
9640 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
9641 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
9642 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
9643 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
9644 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
9645 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9646 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
9647 &motor_DWork.chu_jd);
9648
9649 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
9650 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
9651 if (tmp_0 < 65536.0) {
9652 if (tmp_0 >= 0.0) {
9653 tmp = (uint16_T)tmp_0;
9654 } else {
9655 tmp = 0U;
9656 }
9657 } else {
9658 tmp = MAX_uint16_T;
9659 }
9660
9661 motor_Y.PWMOUT = tmp;
9662 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
9663 if (tmp_0 < 2.147483648E+9) {
9664 if (tmp_0 >= -2.147483648E+9) {
9665 saturatedUnaryMinus = (int32_T)tmp_0;
9666 } else {
9667 saturatedUnaryMinus = MIN_int32_T;
9668 }
9669 } else {
9670 saturatedUnaryMinus = MAX_int32_T;
9671 }
9672
9673 motor_Y.JD_Out = saturatedUnaryMinus;
9674 tmp_0 = motor_B.Motor_XHHY.KP_e;
9675 if (tmp_0 < 2.147483648E+9) {
9676 if (tmp_0 >= -2.147483648E+9) {
9677 saturatedUnaryMinus = (int32_T)tmp_0;
9678 } else {
9679 saturatedUnaryMinus = MIN_int32_T;
9680 }
9681 } else {
9682 saturatedUnaryMinus = MAX_int32_T;
9683 }
9684
9685 motor_Y.JD_Error = saturatedUnaryMinus;
9686 } else if (motor_DWork.temporalCounter_i1 >= (uint32_T)(2.0 / motor_DWork.Ts))
9687 {
9688 /* Transition: '<S1>:465' */
9689 /* 等待2S */
9690 if (motor_DWork.Encode_Pos0 <= 293092.0) {
9691 /* Transition: '<S1>:466' */
9692 /* 293092为2.5度 */
9693 motor_DWork.is_xhhy = motor_IN_P8_e;
9694
9695 /* Entry 'P8': '<S1>:31' */
9696 motor_Y.Flag_Motor_Error = 0U;
9697
9698 /* 电机故障,不能正转
9699 Motor_En=1;
9700 电机失能,关 */
9701 } else {
9702 if (motor_U.Encode_Sp < 0) {
9703 /* Inport: '<Root>/Encode_Sp' */
9704 saturatedUnaryMinus = motor_U.Encode_Sp;
9705 if (saturatedUnaryMinus <= MIN_int32_T) {
9706 saturatedUnaryMinus = MAX_int32_T;
9707 } else {
9708 saturatedUnaryMinus = -saturatedUnaryMinus;
9709 }
9710 } else {
9711 /* Inport: '<Root>/Encode_Sp' */
9712 saturatedUnaryMinus = motor_U.Encode_Sp;
9713 }
9714
9715 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Sp_Min) {
9716 /* Transition: '<S1>:467' */
9717 /* 判断电机是否到达上机械限位
9718 故障(卡死)
9719 编码器故障 */
9720 motor_DWork.is_xhhy = motor_IN_P3_k;
9721
9722 /* Entry 'P3': '<S1>:114' */
9723 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
9724 motor_Y.Flag_Up_GZ_limit = 0U;
9725
9726 /* 上限位开关故障$/
9727 Motor_En=1;
9728 电机失能,关
9729 PWMOUT=PWM_Value_Mid;
9730 4S之后停机$/ */
9731 } else {
9732 guard1 = true;
9733 }
9734 }
9735 } else {
9736 guard1 = true;
9737 }
9738
9739 /* End of Inport: '<Root>/Up_Limit' */
9740 break;
9741
9742 case motor_IN_P10:
9743 /* Inport: '<Root>/Encode_Sp' */
9744 /* During 'P10': '<S1>:30' */
9745 if (motor_U.Encode_Sp < 0) {
9746 saturatedUnaryMinus = motor_U.Encode_Sp;
9747 if (saturatedUnaryMinus <= MIN_int32_T) {
9748 saturatedUnaryMinus = MAX_int32_T;
9749 } else {
9750 saturatedUnaryMinus = -saturatedUnaryMinus;
9751 }
9752 } else {
9753 saturatedUnaryMinus = motor_U.Encode_Sp;
9754 }
9755
9756 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Sp_Min) {
9757 /* Transition: '<S1>:469' */
9758 /* 判断电机是否到达上机械限位 */
9759 motor_DWork.is_xhhy = motor_IN_P12;
9760 motor_DWork.temporalCounter_i1 = 0U;
9761
9762 /* Entry 'P12': '<S1>:1172' */
9763 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
9764 } else {
9765 motor_DWork.chu_jd += 0.003;
9766
9767 /* Inport: '<Root>/JD_In' */
9768 /* 电机上行 */
9769 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
9770 motor_B.JD_In = motor_U.JD_In;
9771 tmp_0 = motor_DWork.Encode_Pos0;
9772 if (tmp_0 < 2.147483648E+9) {
9773 if (tmp_0 >= -2.147483648E+9) {
9774 saturatedUnaryMinus = (int32_T)tmp_0;
9775 } else {
9776 saturatedUnaryMinus = MIN_int32_T;
9777 }
9778 } else {
9779 saturatedUnaryMinus = MAX_int32_T;
9780 }
9781
9782 motor_B.Encode_Pos_c = saturatedUnaryMinus;
9783
9784 /* Inport: '<Root>/Encode_Sp' */
9785 motor_B.Encode_Sp = motor_U.Encode_Sp;
9786
9787 /* Inport: '<Root>/System_Order' */
9788 motor_B.Slect_port = motor_U.System_Order;
9789
9790 /* Inport: '<Root>/SGWY_In' */
9791 motor_B.SGWY_In = motor_U.SGWY_In;
9792
9793 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
9794 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
9795 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
9796 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
9797 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
9798 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
9799 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
9800 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
9801 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9802 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
9803 &motor_DWork.chu_jd);
9804
9805 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
9806 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
9807 if (tmp_0 < 65536.0) {
9808 if (tmp_0 >= 0.0) {
9809 tmp = (uint16_T)tmp_0;
9810 } else {
9811 tmp = 0U;
9812 }
9813 } else {
9814 tmp = MAX_uint16_T;
9815 }
9816
9817 motor_Y.PWMOUT = tmp;
9818 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
9819 if (tmp_0 < 2.147483648E+9) {
9820 if (tmp_0 >= -2.147483648E+9) {
9821 saturatedUnaryMinus = (int32_T)tmp_0;
9822 } else {
9823 saturatedUnaryMinus = MIN_int32_T;
9824 }
9825 } else {
9826 saturatedUnaryMinus = MAX_int32_T;
9827 }
9828
9829 motor_Y.JD_Out = saturatedUnaryMinus;
9830 tmp_0 = motor_B.Motor_XHHY.KP_e;
9831 if (tmp_0 < 2.147483648E+9) {
9832 if (tmp_0 >= -2.147483648E+9) {
9833 saturatedUnaryMinus = (int32_T)tmp_0;
9834 } else {
9835 saturatedUnaryMinus = MIN_int32_T;
9836 }
9837 } else {
9838 saturatedUnaryMinus = MAX_int32_T;
9839 }
9840
9841 motor_Y.JD_Error = saturatedUnaryMinus;
9842
9843 /* 电机触碰到机械上限位 */
9844 }
9845 break;
9846
9847 case motor_IN_P11:
9848 /* During 'P11': '<S1>:34' */
9849 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.2 / motor_DWork.Ts)) {
9850 /* Transition: '<S1>:475' */
9851 motor_DWork.is_xhhy = motor_IN_p10;
9852
9853 /* Entry 'p10': '<S1>:61' */
9854 motor_DWork.Encode_Pos0 = motor_DWork.Encode_Pos2;
9855 motor_DWork.Average_En = 1.0;
9856
9857 /* chu_jd = Angle_Calculation_XH(Encode_Pos); */
9858 motor_DWork.chu_jd = -20.5;
9859 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
9860 } else {
9861 motor_DWork.chu_jd -= 0.003;
9862
9863 /* Inport: '<Root>/JD_In' */
9864 /* 电机下行 */
9865 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
9866 motor_B.JD_In = motor_U.JD_In;
9867 tmp_0 = motor_DWork.Encode_Pos0;
9868 if (tmp_0 < 2.147483648E+9) {
9869 if (tmp_0 >= -2.147483648E+9) {
9870 saturatedUnaryMinus = (int32_T)tmp_0;
9871 } else {
9872 saturatedUnaryMinus = MIN_int32_T;
9873 }
9874 } else {
9875 saturatedUnaryMinus = MAX_int32_T;
9876 }
9877
9878 motor_B.Encode_Pos_c = saturatedUnaryMinus;
9879
9880 /* Inport: '<Root>/Encode_Sp' */
9881 motor_B.Encode_Sp = motor_U.Encode_Sp;
9882
9883 /* Inport: '<Root>/System_Order' */
9884 motor_B.Slect_port = motor_U.System_Order;
9885
9886 /* Inport: '<Root>/SGWY_In' */
9887 motor_B.SGWY_In = motor_U.SGWY_In;
9888
9889 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
9890 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
9891 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
9892 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
9893 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
9894 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
9895 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
9896 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
9897 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9898 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
9899 &motor_DWork.chu_jd);
9900
9901 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
9902 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
9903 if (tmp_0 < 65536.0) {
9904 if (tmp_0 >= 0.0) {
9905 tmp = (uint16_T)tmp_0;
9906 } else {
9907 tmp = 0U;
9908 }
9909 } else {
9910 tmp = MAX_uint16_T;
9911 }
9912
9913 motor_Y.PWMOUT = tmp;
9914 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
9915 if (tmp_0 < 2.147483648E+9) {
9916 if (tmp_0 >= -2.147483648E+9) {
9917 saturatedUnaryMinus = (int32_T)tmp_0;
9918 } else {
9919 saturatedUnaryMinus = MIN_int32_T;
9920 }
9921 } else {
9922 saturatedUnaryMinus = MAX_int32_T;
9923 }
9924
9925 motor_Y.JD_Out = saturatedUnaryMinus;
9926 tmp_0 = motor_B.Motor_XHHY.KP_e;
9927 if (tmp_0 < 2.147483648E+9) {
9928 if (tmp_0 >= -2.147483648E+9) {
9929 saturatedUnaryMinus = (int32_T)tmp_0;
9930 } else {
9931 saturatedUnaryMinus = MIN_int32_T;
9932 }
9933 } else {
9934 saturatedUnaryMinus = MAX_int32_T;
9935 }
9936
9937 motor_Y.JD_Error = saturatedUnaryMinus;
9938
9939 /* 电机触碰到机械限位 */
9940 }
9941 break;
9942
9943 case motor_IN_P12:
9944 /* During 'P12': '<S1>:1172' */
9945 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
9946 /* Transition: '<S1>:1174' */
9947 motor_DWork.is_xhhy = motor_IN_P2_a;
9948 motor_DWork.temporalCounter_i1 = 0U;
9949
9950 /* Entry 'P2': '<S1>:69' */
9951 motor_DWork.chu_jd -= 0.01;
9952
9953 /* Inport: '<Root>/JD_In' */
9954 /* 电机下行 */
9955 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
9956 motor_B.JD_In = motor_U.JD_In;
9957 tmp_0 = motor_DWork.Encode_Pos0;
9958 if (tmp_0 < 2.147483648E+9) {
9959 if (tmp_0 >= -2.147483648E+9) {
9960 saturatedUnaryMinus = (int32_T)tmp_0;
9961 } else {
9962 saturatedUnaryMinus = MIN_int32_T;
9963 }
9964 } else {
9965 saturatedUnaryMinus = MAX_int32_T;
9966 }
9967
9968 motor_B.Encode_Pos_c = saturatedUnaryMinus;
9969
9970 /* Inport: '<Root>/Encode_Sp' */
9971 motor_B.Encode_Sp = motor_U.Encode_Sp;
9972
9973 /* Inport: '<Root>/System_Order' */
9974 motor_B.Slect_port = motor_U.System_Order;
9975
9976 /* Inport: '<Root>/SGWY_In' */
9977 motor_B.SGWY_In = motor_U.SGWY_In;
9978
9979 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
9980 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
9981 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
9982 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
9983 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
9984 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
9985 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
9986 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
9987 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
9988 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
9989 &motor_DWork.chu_jd);
9990
9991 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
9992 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
9993 if (tmp_0 < 65536.0) {
9994 if (tmp_0 >= 0.0) {
9995 tmp = (uint16_T)tmp_0;
9996 } else {
9997 tmp = 0U;
9998 }
9999 } else {
10000 tmp = MAX_uint16_T;
10001 }
10002
10003 motor_Y.PWMOUT = tmp;
10004 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
10005 if (tmp_0 < 2.147483648E+9) {
10006 if (tmp_0 >= -2.147483648E+9) {
10007 saturatedUnaryMinus = (int32_T)tmp_0;
10008 } else {
10009 saturatedUnaryMinus = MIN_int32_T;
10010 }
10011 } else {
10012 saturatedUnaryMinus = MAX_int32_T;
10013 }
10014
10015 motor_Y.JD_Out = saturatedUnaryMinus;
10016 tmp_0 = motor_B.Motor_XHHY.KP_e;
10017 if (tmp_0 < 2.147483648E+9) {
10018 if (tmp_0 >= -2.147483648E+9) {
10019 saturatedUnaryMinus = (int32_T)tmp_0;
10020 } else {
10021 saturatedUnaryMinus = MIN_int32_T;
10022 }
10023 } else {
10024 saturatedUnaryMinus = MAX_int32_T;
10025 }
10026
10027 motor_Y.JD_Error = saturatedUnaryMinus;
10028 }
10029 break;
10030
10031 case motor_IN_P2_a:
10032 /* Inport: '<Root>/Encode_Sp' */
10033 /* Inport: '<Root>/Down_Limit' */
10034 /* During 'P2': '<S1>:69' */
10035 if (motor_U.Encode_Sp < 0) {
10036 saturatedUnaryMinus = motor_U.Encode_Sp;
10037 if (saturatedUnaryMinus <= MIN_int32_T) {
10038 saturatedUnaryMinus = MAX_int32_T;
10039 } else {
10040 saturatedUnaryMinus = -saturatedUnaryMinus;
10041 }
10042 } else {
10043 saturatedUnaryMinus = motor_U.Encode_Sp;
10044 }
10045
10046 if ((motor_DWork.temporalCounter_i1 >= (uint32_T)(3.0 / motor_DWork.Ts)) &&
10047 ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Sp_Min)) {
10048 /* Transition: '<S1>:470' */
10049 /* 等待3S */
10050 /* Transition: '<S1>:471' */
10051 /* 判断电机是否到达下机械限位
10052 故障(卡死) */
10053 motor_DWork.is_xhhy = motor_IN_P5_n;
10054
10055 /* Entry 'P5': '<S1>:33' */
10056 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
10057 motor_Y.Flag_Down_GZ_limit = 0U;
10058
10059 /* 下限位开关故障
10060 Motor_En=1;
10061 PWMOUT=PWM_Value_Mid;
10062 4S之后停机 */
10063 } else if (motor_U.Down_Limit == 0) {
10064 /* Transition: '<S1>:473' */
10065 /* 下限位开关低电平有效 */
10066 motor_DWork.is_xhhy = motor_IN_P4_o;
10067
10068 /* Entry 'P4': '<S1>:38' */
10069 motor_DWork.chu_jd -= 0.003;
10070
10071 /* Inport: '<Root>/JD_In' */
10072 /* 电机下行 */
10073 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
10074 motor_B.JD_In = motor_U.JD_In;
10075 tmp_0 = motor_DWork.Encode_Pos0;
10076 if (tmp_0 < 2.147483648E+9) {
10077 if (tmp_0 >= -2.147483648E+9) {
10078 saturatedUnaryMinus = (int32_T)tmp_0;
10079 } else {
10080 saturatedUnaryMinus = MIN_int32_T;
10081 }
10082 } else {
10083 saturatedUnaryMinus = MAX_int32_T;
10084 }
10085
10086 motor_B.Encode_Pos_c = saturatedUnaryMinus;
10087
10088 /* Inport: '<Root>/Encode_Sp' */
10089 motor_B.Encode_Sp = motor_U.Encode_Sp;
10090
10091 /* Inport: '<Root>/System_Order' */
10092 motor_B.Slect_port = motor_U.System_Order;
10093
10094 /* Inport: '<Root>/SGWY_In' */
10095 motor_B.SGWY_In = motor_U.SGWY_In;
10096
10097 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
10098 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
10099 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
10100 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
10101 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
10102 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
10103 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
10104 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
10105 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
10106 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
10107 &motor_DWork.chu_jd);
10108
10109 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
10110 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
10111 if (tmp_0 < 65536.0) {
10112 if (tmp_0 >= 0.0) {
10113 tmp = (uint16_T)tmp_0;
10114 } else {
10115 tmp = 0U;
10116 }
10117 } else {
10118 tmp = MAX_uint16_T;
10119 }
10120
10121 motor_Y.PWMOUT = tmp;
10122 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
10123 if (tmp_0 < 2.147483648E+9) {
10124 if (tmp_0 >= -2.147483648E+9) {
10125 saturatedUnaryMinus = (int32_T)tmp_0;
10126 } else {
10127 saturatedUnaryMinus = MIN_int32_T;
10128 }
10129 } else {
10130 saturatedUnaryMinus = MAX_int32_T;
10131 }
10132
10133 motor_Y.JD_Out = saturatedUnaryMinus;
10134 tmp_0 = motor_B.Motor_XHHY.KP_e;
10135 if (tmp_0 < 2.147483648E+9) {
10136 if (tmp_0 >= -2.147483648E+9) {
10137 saturatedUnaryMinus = (int32_T)tmp_0;
10138 } else {
10139 saturatedUnaryMinus = MIN_int32_T;
10140 }
10141 } else {
10142 saturatedUnaryMinus = MAX_int32_T;
10143 }
10144
10145 motor_Y.JD_Error = saturatedUnaryMinus;
10146 } else {
10147 motor_DWork.chu_jd -= 0.01;
10148
10149 /* Inport: '<Root>/JD_In' */
10150 /* 电机下行 */
10151 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
10152 motor_B.JD_In = motor_U.JD_In;
10153 tmp_0 = motor_DWork.Encode_Pos0;
10154 if (tmp_0 < 2.147483648E+9) {
10155 if (tmp_0 >= -2.147483648E+9) {
10156 saturatedUnaryMinus = (int32_T)tmp_0;
10157 } else {
10158 saturatedUnaryMinus = MIN_int32_T;
10159 }
10160 } else {
10161 saturatedUnaryMinus = MAX_int32_T;
10162 }
10163
10164 motor_B.Encode_Pos_c = saturatedUnaryMinus;
10165
10166 /* Inport: '<Root>/Encode_Sp' */
10167 motor_B.Encode_Sp = motor_U.Encode_Sp;
10168
10169 /* Inport: '<Root>/System_Order' */
10170 motor_B.Slect_port = motor_U.System_Order;
10171
10172 /* Inport: '<Root>/SGWY_In' */
10173 motor_B.SGWY_In = motor_U.SGWY_In;
10174
10175 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
10176 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
10177 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
10178 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
10179 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
10180 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
10181 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
10182 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
10183 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
10184 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
10185 &motor_DWork.chu_jd);
10186
10187 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
10188 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
10189 if (tmp_0 < 65536.0) {
10190 if (tmp_0 >= 0.0) {
10191 tmp = (uint16_T)tmp_0;
10192 } else {
10193 tmp = 0U;
10194 }
10195 } else {
10196 tmp = MAX_uint16_T;
10197 }
10198
10199 motor_Y.PWMOUT = tmp;
10200 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
10201 if (tmp_0 < 2.147483648E+9) {
10202 if (tmp_0 >= -2.147483648E+9) {
10203 saturatedUnaryMinus = (int32_T)tmp_0;
10204 } else {
10205 saturatedUnaryMinus = MIN_int32_T;
10206 }
10207 } else {
10208 saturatedUnaryMinus = MAX_int32_T;
10209 }
10210
10211 motor_Y.JD_Out = saturatedUnaryMinus;
10212 tmp_0 = motor_B.Motor_XHHY.KP_e;
10213 if (tmp_0 < 2.147483648E+9) {
10214 if (tmp_0 >= -2.147483648E+9) {
10215 saturatedUnaryMinus = (int32_T)tmp_0;
10216 } else {
10217 saturatedUnaryMinus = MIN_int32_T;
10218 }
10219 } else {
10220 saturatedUnaryMinus = MAX_int32_T;
10221 }
10222
10223 motor_Y.JD_Error = saturatedUnaryMinus;
10224 }
10225
10226 /* End of Inport: '<Root>/Down_Limit' */
10227 break;
10228
10229 case motor_IN_P3_k:
10230 /* During 'P3': '<S1>:114' */
10231 /* Transition: '<S1>:1032' */
10232 motor_DWork.is_xhhy = motor_IN_P2_a;
10233 motor_DWork.temporalCounter_i1 = 0U;
10234
10235 /* Entry 'P2': '<S1>:69' */
10236 motor_DWork.chu_jd -= 0.01;
10237
10238 /* Inport: '<Root>/JD_In' */
10239 /* 电机下行 */
10240 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
10241 motor_B.JD_In = motor_U.JD_In;
10242 tmp_0 = motor_DWork.Encode_Pos0;
10243 if (tmp_0 < 2.147483648E+9) {
10244 if (tmp_0 >= -2.147483648E+9) {
10245 saturatedUnaryMinus = (int32_T)tmp_0;
10246 } else {
10247 saturatedUnaryMinus = MIN_int32_T;
10248 }
10249 } else {
10250 saturatedUnaryMinus = MAX_int32_T;
10251 }
10252
10253 motor_B.Encode_Pos_c = saturatedUnaryMinus;
10254
10255 /* Inport: '<Root>/Encode_Sp' */
10256 motor_B.Encode_Sp = motor_U.Encode_Sp;
10257
10258 /* Inport: '<Root>/System_Order' */
10259 motor_B.Slect_port = motor_U.System_Order;
10260
10261 /* Inport: '<Root>/SGWY_In' */
10262 motor_B.SGWY_In = motor_U.SGWY_In;
10263
10264 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
10265 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
10266 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
10267 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
10268 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
10269 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
10270 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
10271 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
10272 &motor_DWork.KG_JD, &motor_DWork.KG_clc, &motor_DWork.P_KP,
10273 &motor_DWork.V_KI, &motor_DWork.V_KP, &motor_DWork.chu_jd);
10274
10275 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
10276 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
10277 if (tmp_0 < 65536.0) {
10278 if (tmp_0 >= 0.0) {
10279 tmp = (uint16_T)tmp_0;
10280 } else {
10281 tmp = 0U;
10282 }
10283 } else {
10284 tmp = MAX_uint16_T;
10285 }
10286
10287 motor_Y.PWMOUT = tmp;
10288 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
10289 if (tmp_0 < 2.147483648E+9) {
10290 if (tmp_0 >= -2.147483648E+9) {
10291 saturatedUnaryMinus = (int32_T)tmp_0;
10292 } else {
10293 saturatedUnaryMinus = MIN_int32_T;
10294 }
10295 } else {
10296 saturatedUnaryMinus = MAX_int32_T;
10297 }
10298
10299 motor_Y.JD_Out = saturatedUnaryMinus;
10300 tmp_0 = motor_B.Motor_XHHY.KP_e;
10301 if (tmp_0 < 2.147483648E+9) {
10302 if (tmp_0 >= -2.147483648E+9) {
10303 saturatedUnaryMinus = (int32_T)tmp_0;
10304 } else {
10305 saturatedUnaryMinus = MIN_int32_T;
10306 }
10307 } else {
10308 saturatedUnaryMinus = MAX_int32_T;
10309 }
10310
10311 motor_Y.JD_Error = saturatedUnaryMinus;
10312 break;
10313
10314 case motor_IN_P4_o:
10315 /* Inport: '<Root>/Encode_Sp' */
10316 /* During 'P4': '<S1>:38' */
10317 if (motor_U.Encode_Sp < 0) {
10318 saturatedUnaryMinus = motor_U.Encode_Sp;
10319 if (saturatedUnaryMinus <= MIN_int32_T) {
10320 saturatedUnaryMinus = MAX_int32_T;
10321 } else {
10322 saturatedUnaryMinus = -saturatedUnaryMinus;
10323 }
10324 } else {
10325 saturatedUnaryMinus = motor_U.Encode_Sp;
10326 }
10327
10328 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Sp_Min) {
10329 /* Transition: '<S1>:474' */
10330 /* 判断电机是否到达下机械限位 */
10331 motor_DWork.is_xhhy = motor_IN_P11;
10332 motor_DWork.temporalCounter_i1 = 0U;
10333
10334 /* Entry 'P11': '<S1>:34' */
10335 motor_DWork.chu_jd -= 0.003;
10336
10337 /* Inport: '<Root>/JD_In' */
10338 /* 电机下行 */
10339 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
10340 motor_B.JD_In = motor_U.JD_In;
10341 tmp_0 = motor_DWork.Encode_Pos0;
10342 if (tmp_0 < 2.147483648E+9) {
10343 if (tmp_0 >= -2.147483648E+9) {
10344 saturatedUnaryMinus = (int32_T)tmp_0;
10345 } else {
10346 saturatedUnaryMinus = MIN_int32_T;
10347 }
10348 } else {
10349 saturatedUnaryMinus = MAX_int32_T;
10350 }
10351
10352 motor_B.Encode_Pos_c = saturatedUnaryMinus;
10353
10354 /* Inport: '<Root>/Encode_Sp' */
10355 motor_B.Encode_Sp = motor_U.Encode_Sp;
10356
10357 /* Inport: '<Root>/System_Order' */
10358 motor_B.Slect_port = motor_U.System_Order;
10359
10360 /* Inport: '<Root>/SGWY_In' */
10361 motor_B.SGWY_In = motor_U.SGWY_In;
10362
10363 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
10364 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
10365 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
10366 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
10367 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
10368 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
10369 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
10370 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
10371 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
10372 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
10373 &motor_DWork.chu_jd);
10374
10375 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
10376 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
10377 if (tmp_0 < 65536.0) {
10378 if (tmp_0 >= 0.0) {
10379 tmp = (uint16_T)tmp_0;
10380 } else {
10381 tmp = 0U;
10382 }
10383 } else {
10384 tmp = MAX_uint16_T;
10385 }
10386
10387 motor_Y.PWMOUT = tmp;
10388 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
10389 if (tmp_0 < 2.147483648E+9) {
10390 if (tmp_0 >= -2.147483648E+9) {
10391 saturatedUnaryMinus = (int32_T)tmp_0;
10392 } else {
10393 saturatedUnaryMinus = MIN_int32_T;
10394 }
10395 } else {
10396 saturatedUnaryMinus = MAX_int32_T;
10397 }
10398
10399 motor_Y.JD_Out = saturatedUnaryMinus;
10400 tmp_0 = motor_B.Motor_XHHY.KP_e;
10401 if (tmp_0 < 2.147483648E+9) {
10402 if (tmp_0 >= -2.147483648E+9) {
10403 saturatedUnaryMinus = (int32_T)tmp_0;
10404 } else {
10405 saturatedUnaryMinus = MIN_int32_T;
10406 }
10407 } else {
10408 saturatedUnaryMinus = MAX_int32_T;
10409 }
10410
10411 motor_Y.JD_Error = saturatedUnaryMinus;
10412 } else {
10413 motor_DWork.chu_jd -= 0.003;
10414
10415 /* Inport: '<Root>/JD_In' */
10416 /* 电机下行 */
10417 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
10418 motor_B.JD_In = motor_U.JD_In;
10419 tmp_0 = motor_DWork.Encode_Pos0;
10420 if (tmp_0 < 2.147483648E+9) {
10421 if (tmp_0 >= -2.147483648E+9) {
10422 saturatedUnaryMinus = (int32_T)tmp_0;
10423 } else {
10424 saturatedUnaryMinus = MIN_int32_T;
10425 }
10426 } else {
10427 saturatedUnaryMinus = MAX_int32_T;
10428 }
10429
10430 motor_B.Encode_Pos_c = saturatedUnaryMinus;
10431
10432 /* Inport: '<Root>/Encode_Sp' */
10433 motor_B.Encode_Sp = motor_U.Encode_Sp;
10434
10435 /* Inport: '<Root>/System_Order' */
10436 motor_B.Slect_port = motor_U.System_Order;
10437
10438 /* Inport: '<Root>/SGWY_In' */
10439 motor_B.SGWY_In = motor_U.SGWY_In;
10440
10441 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
10442 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
10443 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
10444 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
10445 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
10446 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
10447 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
10448 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
10449 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
10450 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
10451 &motor_DWork.chu_jd);
10452
10453 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
10454 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
10455 if (tmp_0 < 65536.0) {
10456 if (tmp_0 >= 0.0) {
10457 tmp = (uint16_T)tmp_0;
10458 } else {
10459 tmp = 0U;
10460 }
10461 } else {
10462 tmp = MAX_uint16_T;
10463 }
10464
10465 motor_Y.PWMOUT = tmp;
10466 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
10467 if (tmp_0 < 2.147483648E+9) {
10468 if (tmp_0 >= -2.147483648E+9) {
10469 saturatedUnaryMinus = (int32_T)tmp_0;
10470 } else {
10471 saturatedUnaryMinus = MIN_int32_T;
10472 }
10473 } else {
10474 saturatedUnaryMinus = MAX_int32_T;
10475 }
10476
10477 motor_Y.JD_Out = saturatedUnaryMinus;
10478 tmp_0 = motor_B.Motor_XHHY.KP_e;
10479 if (tmp_0 < 2.147483648E+9) {
10480 if (tmp_0 >= -2.147483648E+9) {
10481 saturatedUnaryMinus = (int32_T)tmp_0;
10482 } else {
10483 saturatedUnaryMinus = MIN_int32_T;
10484 }
10485 } else {
10486 saturatedUnaryMinus = MAX_int32_T;
10487 }
10488
10489 motor_Y.JD_Error = saturatedUnaryMinus;
10490
10491 /* 电机触碰到机械限位 */
10492 }
10493 break;
10494
10495 case motor_IN_P5_n:
10496 /* During 'P5': '<S1>:33' */
10497 /* Transition: '<S1>:1033' */
10498 motor_DWork.is_xhhy = motor_IN_P11;
10499 motor_DWork.temporalCounter_i1 = 0U;
10500
10501 /* Entry 'P11': '<S1>:34' */
10502 motor_DWork.chu_jd -= 0.003;
10503
10504 /* Inport: '<Root>/JD_In' */
10505 /* 电机下行 */
10506 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
10507 motor_B.JD_In = motor_U.JD_In;
10508 tmp_0 = motor_DWork.Encode_Pos0;
10509 if (tmp_0 < 2.147483648E+9) {
10510 if (tmp_0 >= -2.147483648E+9) {
10511 saturatedUnaryMinus = (int32_T)tmp_0;
10512 } else {
10513 saturatedUnaryMinus = MIN_int32_T;
10514 }
10515 } else {
10516 saturatedUnaryMinus = MAX_int32_T;
10517 }
10518
10519 motor_B.Encode_Pos_c = saturatedUnaryMinus;
10520
10521 /* Inport: '<Root>/Encode_Sp' */
10522 motor_B.Encode_Sp = motor_U.Encode_Sp;
10523
10524 /* Inport: '<Root>/System_Order' */
10525 motor_B.Slect_port = motor_U.System_Order;
10526
10527 /* Inport: '<Root>/SGWY_In' */
10528 motor_B.SGWY_In = motor_U.SGWY_In;
10529
10530 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
10531 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
10532 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
10533 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
10534 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
10535 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
10536 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
10537 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
10538 &motor_DWork.KG_JD, &motor_DWork.KG_clc, &motor_DWork.P_KP,
10539 &motor_DWork.V_KI, &motor_DWork.V_KP, &motor_DWork.chu_jd);
10540
10541 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
10542 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
10543 if (tmp_0 < 65536.0) {
10544 if (tmp_0 >= 0.0) {
10545 tmp = (uint16_T)tmp_0;
10546 } else {
10547 tmp = 0U;
10548 }
10549 } else {
10550 tmp = MAX_uint16_T;
10551 }
10552
10553 motor_Y.PWMOUT = tmp;
10554 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
10555 if (tmp_0 < 2.147483648E+9) {
10556 if (tmp_0 >= -2.147483648E+9) {
10557 saturatedUnaryMinus = (int32_T)tmp_0;
10558 } else {
10559 saturatedUnaryMinus = MIN_int32_T;
10560 }
10561 } else {
10562 saturatedUnaryMinus = MAX_int32_T;
10563 }
10564
10565 motor_Y.JD_Out = saturatedUnaryMinus;
10566 tmp_0 = motor_B.Motor_XHHY.KP_e;
10567 if (tmp_0 < 2.147483648E+9) {
10568 if (tmp_0 >= -2.147483648E+9) {
10569 saturatedUnaryMinus = (int32_T)tmp_0;
10570 } else {
10571 saturatedUnaryMinus = MIN_int32_T;
10572 }
10573 } else {
10574 saturatedUnaryMinus = MAX_int32_T;
10575 }
10576
10577 motor_Y.JD_Error = saturatedUnaryMinus;
10578 break;
10579
10580 case motor_IN_P8_e:
10581 /* During 'P8': '<S1>:31' */
10582 break;
10583
10584 case motor_IN_p10:
10585 /* During 'p10': '<S1>:61' */
10586 if (fabs(motor_DWork.chu_jd) <= 0.02) {
10587 /* Transition: '<S1>:476' */
10588 motor_DWork.is_xhhy = motor_IN_p11_b;
10589 motor_DWork.temporalCounter_i1 = 0U;
10590
10591 /* Entry 'p11': '<S1>:113' */
10592 motor_Y.Motor_En = true;
10593
10594 /* 关电机 */
10595 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
10596 } else {
10597 motor_DWork.chu_jd += 0.01;
10598
10599 /* Inport: '<Root>/JD_In' */
10600 /* 电机上行回零点 */
10601 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
10602 motor_B.JD_In = motor_U.JD_In;
10603 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
10604
10605 /* Inport: '<Root>/Encode_Sp' */
10606 motor_B.Encode_Sp = motor_U.Encode_Sp;
10607
10608 /* Inport: '<Root>/System_Order' */
10609 motor_B.Slect_port = motor_U.System_Order;
10610
10611 /* Inport: '<Root>/SGWY_In' */
10612 motor_B.SGWY_In = motor_U.SGWY_In;
10613
10614 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
10615 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
10616 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
10617 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
10618 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
10619 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
10620 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
10621 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
10622 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
10623 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
10624 &motor_DWork.chu_jd);
10625
10626 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
10627 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
10628 if (tmp_0 < 65536.0) {
10629 if (tmp_0 >= 0.0) {
10630 tmp = (uint16_T)tmp_0;
10631 } else {
10632 tmp = 0U;
10633 }
10634 } else {
10635 tmp = MAX_uint16_T;
10636 }
10637
10638 motor_Y.PWMOUT = tmp;
10639 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
10640 if (tmp_0 < 2.147483648E+9) {
10641 if (tmp_0 >= -2.147483648E+9) {
10642 saturatedUnaryMinus = (int32_T)tmp_0;
10643 } else {
10644 saturatedUnaryMinus = MIN_int32_T;
10645 }
10646 } else {
10647 saturatedUnaryMinus = MAX_int32_T;
10648 }
10649
10650 motor_Y.JD_Out = saturatedUnaryMinus;
10651 tmp_0 = motor_B.Motor_XHHY.KP_e;
10652 if (tmp_0 < 2.147483648E+9) {
10653 if (tmp_0 >= -2.147483648E+9) {
10654 saturatedUnaryMinus = (int32_T)tmp_0;
10655 } else {
10656 saturatedUnaryMinus = MIN_int32_T;
10657 }
10658 } else {
10659 saturatedUnaryMinus = MAX_int32_T;
10660 }
10661
10662 motor_Y.JD_Error = saturatedUnaryMinus;
10663 }
10664 break;
10665
10666 default:
10667 /* During 'p11': '<S1>:113' */
10668 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.2 / motor_DWork.Ts)) {
10669 /* Transition: '<S1>:477' */
10670 /* Transition: '<S1>:406' */
10671 motor_DWork.is_xhhy = motor_IN_NO_ACTIVE_CHILD;
10672 motor_DWork.is_Initialize = motor_IN_Check_Reset;
10673
10674 /* Entry 'Check_Reset': '<S1>:71' */
10675 motor_Y.Ini_Result = motor_Init_Result_Check(motor_Y.Flag_Cur,
10676 motor_Y.Flag_Temp_Up, (real_T)motor_Y.Flag_Temp_Down);
10677 motor_Y.DCZD = true;
10678 }
10679 break;
10680 }
10681
10682 if (guard1) {
10683 motor_DWork.chu_jd += 0.01;
10684
10685 /* Inport: '<Root>/JD_In' */
10686 /* 电机上行 */
10687 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
10688 motor_B.JD_In = motor_U.JD_In;
10689 tmp_0 = motor_DWork.Encode_Pos0;
10690 if (tmp_0 < 2.147483648E+9) {
10691 if (tmp_0 >= -2.147483648E+9) {
10692 saturatedUnaryMinus = (int32_T)tmp_0;
10693 } else {
10694 saturatedUnaryMinus = MIN_int32_T;
10695 }
10696 } else {
10697 saturatedUnaryMinus = MAX_int32_T;
10698 }
10699
10700 motor_B.Encode_Pos_c = saturatedUnaryMinus;
10701
10702 /* Inport: '<Root>/Encode_Sp' */
10703 motor_B.Encode_Sp = motor_U.Encode_Sp;
10704
10705 /* Inport: '<Root>/System_Order' */
10706 motor_B.Slect_port = motor_U.System_Order;
10707
10708 /* Inport: '<Root>/SGWY_In' */
10709 motor_B.SGWY_In = motor_U.SGWY_In;
10710
10711 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
10712 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
10713 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
10714 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
10715 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
10716 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
10717 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
10718 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
10719 &motor_DWork.KG_JD, &motor_DWork.KG_clc, &motor_DWork.P_KP,
10720 &motor_DWork.V_KI, &motor_DWork.V_KP, &motor_DWork.chu_jd);
10721
10722 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
10723 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
10724 if (tmp_0 < 65536.0) {
10725 if (tmp_0 >= 0.0) {
10726 tmp = (uint16_T)tmp_0;
10727 } else {
10728 tmp = 0U;
10729 }
10730 } else {
10731 tmp = MAX_uint16_T;
10732 }
10733
10734 motor_Y.PWMOUT = tmp;
10735 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
10736 if (tmp_0 < 2.147483648E+9) {
10737 if (tmp_0 >= -2.147483648E+9) {
10738 saturatedUnaryMinus = (int32_T)tmp_0;
10739 } else {
10740 saturatedUnaryMinus = MIN_int32_T;
10741 }
10742 } else {
10743 saturatedUnaryMinus = MAX_int32_T;
10744 }
10745
10746 motor_Y.JD_Out = saturatedUnaryMinus;
10747 tmp_0 = motor_B.Motor_XHHY.KP_e;
10748 if (tmp_0 < 2.147483648E+9) {
10749 if (tmp_0 >= -2.147483648E+9) {
10750 saturatedUnaryMinus = (int32_T)tmp_0;
10751 } else {
10752 saturatedUnaryMinus = MIN_int32_T;
10753 }
10754 } else {
10755 saturatedUnaryMinus = MAX_int32_T;
10756 }
10757
10758 motor_Y.JD_Error = saturatedUnaryMinus;
10759 }
10760}
10761
10762/* Function for Chart: '<Root>/Chart' */
10763static void motor_Initialize(void)
10764{
10765 boolean_T guard1 = false;
10766 boolean_T guard2 = false;
10767 uint16_T tmp;
10768 real_T tmp_0;
10769 int32_T saturatedUnaryMinus;
10770
10771 /* During 'Initialize': '<S1>:12' */
10772 switch (motor_DWork.is_Initialize) {
10773 case motor_IN_Check_Reset:
10774 /* Inport: '<Root>/System_Order' */
10775 /* During 'Check_Reset': '<S1>:71' */
10776 if ((motor_Y.Ini_Result == 1) && (motor_U.System_Order == 2)) {
10777 /* Inport: '<Root>/Working_Mode' */
10778 /* Transition: '<S1>:402' */
10779 /* 代表初始化成功,接收开机指令 */
10780 if (motor_U.Working_Mode == 3) {
10781 /* Transition: '<S1>:387' */
10782 motor_DWork.is_Initialize = motor_IN_NO_ACTIVE_CHILD;
10783 motor_DWork.is_M_Run = motor_IN_Normal_Mode;
10784
10785 /* Entry 'Normal_Mode': '<S1>:13' */
10786 /* 正常模式 */
10787 motor_Y.DCZD = false;
10788
10789 /* 开电磁制动,低有效 */
10790 motor_Y.Open_Result = 3U;
10791
10792 /* 开机未完成状态位 */
10793 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
10794
10795 /* Entry Internal 'Normal_Mode': '<S1>:13' */
10796 /* Transition: '<S1>:410' */
10797 motor_DWork.is_Normal_Mode = motor_IN_defult1;
10798 motor_DWork.temporalCounter_i1 = 0U;
10799 } else if (motor_U.Working_Mode == 2) {
10800 /* Transition: '<S1>:385' */
10801 motor_DWork.is_Initialize = motor_IN_NO_ACTIVE_CHILD;
10802 motor_DWork.is_M_Run = motor_IN_Showing_Mode;
10803
10804 /* Entry 'Showing_Mode': '<S1>:51' */
10805 /* 检视模式 */
10806 motor_Y.DCZD = false;
10807
10808 /* 开电磁制动,低有效 */
10809 motor_Y.Open_Result = 3U;
10810
10811 /* 开机未完成状态位 */
10812 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
10813 motor_DWork.KG = 0U;
10814
10815 /* Entry Internal 'Showing_Mode': '<S1>:51' */
10816 /* Transition: '<S1>:437' */
10817 motor_DWork.is_Showing_Mode = motor_IN_Start;
10818 motor_DWork.temporalCounter_i1 = 0U;
10819 } else if (motor_U.Working_Mode == 1) {
10820 /* Transition: '<S1>:382' */
10821 motor_DWork.is_Initialize = motor_IN_NO_ACTIVE_CHILD;
10822 motor_DWork.is_M_Run = motor_IN_Test_Mode;
10823
10824 /* Entry 'Test_Mode': '<S1>:35' */
10825 /* 调试模式 */
10826 /* Entry Internal 'Test_Mode': '<S1>:35' */
10827 /* Transition: '<S1>:424' */
10828 motor_DWork.is_Test_Mode = motor_IN_defult;
10829
10830 /* Entry 'defult': '<S1>:239' */
10831 motor_Y.Open_Result = 1U;
10832
10833 /* 开机状态位成功 */
10834 motor_DWork.In_State = 2U;
10835 } else {
10836 guard2 = true;
10837 }
10838
10839 /* End of Inport: '<Root>/Working_Mode' */
10840 } else {
10841 guard2 = true;
10842 }
10843 break;
10844
10845 case motor_IN_Parameters_Reset:
10846 /* Inport: '<Root>/Motor_Num' */
10847 /* During 'Parameters_Reset': '<S1>:37' */
10848 /* Transition: '<S1>:400' */
10849 if (motor_U.Motor_Num == 1) {
10850 /* Transition: '<S1>:403' */
10851 motor_DWork.is_Initialize = motor_IN_xhzy;
10852
10853 /* Entry Internal 'xhzy': '<S1>:128' */
10854 /* Transition: '<S1>:484' */
10855 motor_DWork.is_xhzy = motor_IN_P1;
10856 motor_DWork.temporalCounter_i1 = 0U;
10857
10858 /* Entry 'P1': '<S1>:66' */
10859 motor_DWork.KG_JD = 0U;
10860 motor_DWork.KG_En = 1U;
10861 motor_DWork.KG_clc = 0U;
10862 motor_DWork.chu_jd += 0.01;
10863
10864 /* Inport: '<Root>/JD_In' */
10865 /* 电机上行 */
10866 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
10867 motor_B.JD_In = motor_U.JD_In;
10868 tmp_0 = motor_DWork.Encode_Pos0;
10869 if (tmp_0 < 2.147483648E+9) {
10870 if (tmp_0 >= -2.147483648E+9) {
10871 saturatedUnaryMinus = (int32_T)tmp_0;
10872 } else {
10873 saturatedUnaryMinus = MIN_int32_T;
10874 }
10875 } else {
10876 saturatedUnaryMinus = MAX_int32_T;
10877 }
10878
10879 motor_B.Encode_Pos_c = saturatedUnaryMinus;
10880
10881 /* Inport: '<Root>/Encode_Sp' */
10882 motor_B.Encode_Sp = motor_U.Encode_Sp;
10883
10884 /* Inport: '<Root>/System_Order' */
10885 motor_B.Slect_port = motor_U.System_Order;
10886
10887 /* Inport: '<Root>/SGWY_In' */
10888 motor_B.SGWY_In = motor_U.SGWY_In;
10889
10890 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
10891 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
10892 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
10893 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
10894 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
10895 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
10896 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
10897 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
10898 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
10899 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
10900 &motor_DWork.chu_jd);
10901
10902 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
10903 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
10904 if (tmp_0 < 65536.0) {
10905 if (tmp_0 >= 0.0) {
10906 tmp = (uint16_T)tmp_0;
10907 } else {
10908 tmp = 0U;
10909 }
10910 } else {
10911 tmp = MAX_uint16_T;
10912 }
10913
10914 motor_Y.PWMOUT = tmp;
10915 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
10916 if (tmp_0 < 2.147483648E+9) {
10917 if (tmp_0 >= -2.147483648E+9) {
10918 saturatedUnaryMinus = (int32_T)tmp_0;
10919 } else {
10920 saturatedUnaryMinus = MIN_int32_T;
10921 }
10922 } else {
10923 saturatedUnaryMinus = MAX_int32_T;
10924 }
10925
10926 motor_Y.JD_Out = saturatedUnaryMinus;
10927 tmp_0 = motor_B.Motor_XHHY.KP_e;
10928 if (tmp_0 < 2.147483648E+9) {
10929 if (tmp_0 >= -2.147483648E+9) {
10930 saturatedUnaryMinus = (int32_T)tmp_0;
10931 } else {
10932 saturatedUnaryMinus = MIN_int32_T;
10933 }
10934 } else {
10935 saturatedUnaryMinus = MAX_int32_T;
10936 }
10937
10938 motor_Y.JD_Error = saturatedUnaryMinus;
10939 } else if (motor_U.Motor_Num == 3) {
10940 /* Transition: '<S1>:397' */
10941 motor_DWork.is_Initialize = motor_IN_xhhy;
10942
10943 /* Entry Internal 'xhhy': '<S1>:65' */
10944 /* Transition: '<S1>:464' */
10945 motor_DWork.is_xhhy = motor_IN_P1;
10946 motor_DWork.temporalCounter_i1 = 0U;
10947
10948 /* Entry 'P1': '<S1>:25' */
10949 motor_DWork.KG_JD = 0U;
10950 motor_DWork.KG_En = 1U;
10951 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
10952 motor_DWork.chu_jd += 0.01;
10953
10954 /* Inport: '<Root>/JD_In' */
10955 /* 电机上行 */
10956 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
10957 motor_B.JD_In = motor_U.JD_In;
10958 tmp_0 = motor_DWork.Encode_Pos0;
10959 if (tmp_0 < 2.147483648E+9) {
10960 if (tmp_0 >= -2.147483648E+9) {
10961 saturatedUnaryMinus = (int32_T)tmp_0;
10962 } else {
10963 saturatedUnaryMinus = MIN_int32_T;
10964 }
10965 } else {
10966 saturatedUnaryMinus = MAX_int32_T;
10967 }
10968
10969 motor_B.Encode_Pos_c = saturatedUnaryMinus;
10970
10971 /* Inport: '<Root>/Encode_Sp' */
10972 motor_B.Encode_Sp = motor_U.Encode_Sp;
10973
10974 /* Inport: '<Root>/System_Order' */
10975 motor_B.Slect_port = motor_U.System_Order;
10976
10977 /* Inport: '<Root>/SGWY_In' */
10978 motor_B.SGWY_In = motor_U.SGWY_In;
10979
10980 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
10981 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
10982 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
10983 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
10984 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
10985 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
10986 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
10987 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
10988 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
10989 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
10990 &motor_DWork.chu_jd);
10991
10992 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
10993 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
10994 if (tmp_0 < 65536.0) {
10995 if (tmp_0 >= 0.0) {
10996 tmp = (uint16_T)tmp_0;
10997 } else {
10998 tmp = 0U;
10999 }
11000 } else {
11001 tmp = MAX_uint16_T;
11002 }
11003
11004 motor_Y.PWMOUT = tmp;
11005 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
11006 if (tmp_0 < 2.147483648E+9) {
11007 if (tmp_0 >= -2.147483648E+9) {
11008 saturatedUnaryMinus = (int32_T)tmp_0;
11009 } else {
11010 saturatedUnaryMinus = MIN_int32_T;
11011 }
11012 } else {
11013 saturatedUnaryMinus = MAX_int32_T;
11014 }
11015
11016 motor_Y.JD_Out = saturatedUnaryMinus;
11017 tmp_0 = motor_B.Motor_XHHY.KP_e;
11018 if (tmp_0 < 2.147483648E+9) {
11019 if (tmp_0 >= -2.147483648E+9) {
11020 saturatedUnaryMinus = (int32_T)tmp_0;
11021 } else {
11022 saturatedUnaryMinus = MIN_int32_T;
11023 }
11024 } else {
11025 saturatedUnaryMinus = MAX_int32_T;
11026 }
11027
11028 motor_Y.JD_Error = saturatedUnaryMinus;
11029 } else {
11030 if (motor_U.Motor_Num == 2) {
11031 /* Transition: '<S1>:401' */
11032 motor_DWork.is_Initialize = motor_IN_hy;
11033
11034 /* Entry Internal 'hy': '<S1>:62' */
11035 /* Transition: '<S1>:451' */
11036 motor_DWork.is_hy = motor_IN_P1;
11037 motor_DWork.temporalCounter_i1 = 0U;
11038
11039 /* Entry 'P1': '<S1>:39' */
11040 motor_DWork.KG_JD = 0U;
11041 motor_DWork.KG_clc = 0U;
11042 }
11043 }
11044
11045 /* End of Inport: '<Root>/Motor_Num' */
11046 break;
11047
11048 case motor_IN_Parameters_Reset0:
11049 /* During 'Parameters_Reset0': '<S1>:89' */
11050 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.2 / motor_DWork.Ts)) {
11051 /* Transition: '<S1>:399' */
11052 motor_DWork.is_Initialize = motor_IN_Parameters_Reset;
11053
11054 /* Entry 'Parameters_Reset': '<S1>:37' */
11055 motor_Y.Motor_En = false;
11056
11057 /* 开电机 */
11058 motor_DWork.Encode_Pos0 = 0.0;
11059
11060 /* 位置初始值 */
11061 motor_DWork.chu_jd = 0.0;
11062 motor_DWork.KG = 1U;
11063 }
11064 break;
11065
11066 case motor_IN_hy:
11067 motor_hy();
11068 break;
11069
11070 case motor_IN_xhhy:
11071 motor_xhhy();
11072 break;
11073
11074 default:
11075 /* During 'xhzy': '<S1>:128' */
11076 switch (motor_DWork.is_xhzy) {
11077 case motor_IN_P1:
11078 /* During 'P1': '<S1>:66' */
11079 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(2.0 / motor_DWork.Ts)) {
11080 /* Transition: '<S1>:485' */
11081 /* 等待2S */
11082 if (motor_DWork.Encode_Pos0 <= 293092.0) {
11083 /* Transition: '<S1>:486' */
11084 /* 293092为2.5度 */
11085 motor_DWork.is_xhzy = motor_IN_P8;
11086
11087 /* Entry 'P8': '<S1>:32' */
11088 motor_Y.Flag_Motor_Error = 0U;
11089
11090 /* 电机故障,不能正转 */
11091 motor_Y.Motor_En = true;
11092
11093 /* 电机失能,关 */
11094 } else {
11095 if (motor_U.Encode_Sp < 0) {
11096 /* Inport: '<Root>/Encode_Sp' */
11097 saturatedUnaryMinus = motor_U.Encode_Sp;
11098 if (saturatedUnaryMinus <= MIN_int32_T) {
11099 saturatedUnaryMinus = MAX_int32_T;
11100 } else {
11101 saturatedUnaryMinus = -saturatedUnaryMinus;
11102 }
11103 } else {
11104 /* Inport: '<Root>/Encode_Sp' */
11105 saturatedUnaryMinus = motor_U.Encode_Sp;
11106 }
11107
11108 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Sp_Min) {
11109 /* Transition: '<S1>:487' */
11110 /* 判断电机是否到达上机械限位
11111 故障(卡死)
11112 编码器故障 */
11113 motor_DWork.is_xhzy = motor_IN_P3;
11114
11115 /* Entry 'P3': '<S1>:127' */
11116 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
11117 motor_Y.Flag_Up_GZ_limit = 0U;
11118
11119 /* 上限位开关故障$/
11120 Motor_En=1;
11121 电机失能,关
11122 PWMOUT=PWM_Value_Mid;
11123 4S之后停机$/ */
11124 } else {
11125 guard1 = true;
11126 }
11127 }
11128 } else {
11129 guard1 = true;
11130 }
11131 break;
11132
11133 case motor_IN_P10:
11134 /* During 'P10': '<S1>:63' */
11135 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.2 / motor_DWork.Ts)) {
11136 /* Transition: '<S1>:495' */
11137 motor_DWork.is_xhzy = motor_IN_p11;
11138
11139 /* Entry 'p11': '<S1>:126' */
11140 motor_DWork.Encode_Pos0 = motor_DWork.Encode_Pos1;
11141 motor_DWork.Average_En = 1.0;
11142
11143 /* chu_jd = Angle_Calculation_XH(Encode_Pos); */
11144 motor_DWork.chu_jd = -20.5;
11145 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
11146 } else {
11147 motor_DWork.chu_jd -= 0.003;
11148
11149 /* Inport: '<Root>/JD_In' */
11150 /* 电机下行; */
11151 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
11152 motor_B.JD_In = motor_U.JD_In;
11153 tmp_0 = motor_DWork.Encode_Pos0;
11154 if (tmp_0 < 2.147483648E+9) {
11155 if (tmp_0 >= -2.147483648E+9) {
11156 saturatedUnaryMinus = (int32_T)tmp_0;
11157 } else {
11158 saturatedUnaryMinus = MIN_int32_T;
11159 }
11160 } else {
11161 saturatedUnaryMinus = MAX_int32_T;
11162 }
11163
11164 motor_B.Encode_Pos_c = saturatedUnaryMinus;
11165
11166 /* Inport: '<Root>/Encode_Sp' */
11167 motor_B.Encode_Sp = motor_U.Encode_Sp;
11168
11169 /* Inport: '<Root>/System_Order' */
11170 motor_B.Slect_port = motor_U.System_Order;
11171
11172 /* Inport: '<Root>/SGWY_In' */
11173 motor_B.SGWY_In = motor_U.SGWY_In;
11174
11175 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
11176 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
11177 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
11178 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
11179 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
11180 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
11181 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
11182 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
11183 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
11184 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
11185 &motor_DWork.chu_jd);
11186
11187 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
11188 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
11189 if (tmp_0 < 65536.0) {
11190 if (tmp_0 >= 0.0) {
11191 tmp = (uint16_T)tmp_0;
11192 } else {
11193 tmp = 0U;
11194 }
11195 } else {
11196 tmp = MAX_uint16_T;
11197 }
11198
11199 motor_Y.PWMOUT = tmp;
11200 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
11201 if (tmp_0 < 2.147483648E+9) {
11202 if (tmp_0 >= -2.147483648E+9) {
11203 saturatedUnaryMinus = (int32_T)tmp_0;
11204 } else {
11205 saturatedUnaryMinus = MIN_int32_T;
11206 }
11207 } else {
11208 saturatedUnaryMinus = MAX_int32_T;
11209 }
11210
11211 motor_Y.JD_Out = saturatedUnaryMinus;
11212 tmp_0 = motor_B.Motor_XHHY.KP_e;
11213 if (tmp_0 < 2.147483648E+9) {
11214 if (tmp_0 >= -2.147483648E+9) {
11215 saturatedUnaryMinus = (int32_T)tmp_0;
11216 } else {
11217 saturatedUnaryMinus = MIN_int32_T;
11218 }
11219 } else {
11220 saturatedUnaryMinus = MAX_int32_T;
11221 }
11222
11223 motor_Y.JD_Error = saturatedUnaryMinus;
11224
11225 /* 电机下行
11226 电机触碰到机械限位 */
11227 }
11228 break;
11229
11230 case motor_IN_P11:
11231 /* During 'P11': '<S1>:1170' */
11232 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
11233 /* Transition: '<S1>:1171' */
11234 motor_DWork.is_xhzy = motor_IN_P2;
11235 motor_DWork.temporalCounter_i1 = 0U;
11236
11237 /* Entry 'P2': '<S1>:67' */
11238 motor_DWork.chu_jd -= 0.01;
11239
11240 /* Inport: '<Root>/JD_In' */
11241 /* 电机下行; */
11242 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
11243 motor_B.JD_In = motor_U.JD_In;
11244 tmp_0 = motor_DWork.Encode_Pos0;
11245 if (tmp_0 < 2.147483648E+9) {
11246 if (tmp_0 >= -2.147483648E+9) {
11247 saturatedUnaryMinus = (int32_T)tmp_0;
11248 } else {
11249 saturatedUnaryMinus = MIN_int32_T;
11250 }
11251 } else {
11252 saturatedUnaryMinus = MAX_int32_T;
11253 }
11254
11255 motor_B.Encode_Pos_c = saturatedUnaryMinus;
11256
11257 /* Inport: '<Root>/Encode_Sp' */
11258 motor_B.Encode_Sp = motor_U.Encode_Sp;
11259
11260 /* Inport: '<Root>/System_Order' */
11261 motor_B.Slect_port = motor_U.System_Order;
11262
11263 /* Inport: '<Root>/SGWY_In' */
11264 motor_B.SGWY_In = motor_U.SGWY_In;
11265
11266 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
11267 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
11268 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
11269 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
11270 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
11271 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
11272 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
11273 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
11274 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
11275 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
11276 &motor_DWork.chu_jd);
11277
11278 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
11279 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
11280 if (tmp_0 < 65536.0) {
11281 if (tmp_0 >= 0.0) {
11282 tmp = (uint16_T)tmp_0;
11283 } else {
11284 tmp = 0U;
11285 }
11286 } else {
11287 tmp = MAX_uint16_T;
11288 }
11289
11290 motor_Y.PWMOUT = tmp;
11291 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
11292 if (tmp_0 < 2.147483648E+9) {
11293 if (tmp_0 >= -2.147483648E+9) {
11294 saturatedUnaryMinus = (int32_T)tmp_0;
11295 } else {
11296 saturatedUnaryMinus = MIN_int32_T;
11297 }
11298 } else {
11299 saturatedUnaryMinus = MAX_int32_T;
11300 }
11301
11302 motor_Y.JD_Out = saturatedUnaryMinus;
11303 tmp_0 = motor_B.Motor_XHHY.KP_e;
11304 if (tmp_0 < 2.147483648E+9) {
11305 if (tmp_0 >= -2.147483648E+9) {
11306 saturatedUnaryMinus = (int32_T)tmp_0;
11307 } else {
11308 saturatedUnaryMinus = MIN_int32_T;
11309 }
11310 } else {
11311 saturatedUnaryMinus = MAX_int32_T;
11312 }
11313
11314 motor_Y.JD_Error = saturatedUnaryMinus;
11315 }
11316 break;
11317
11318 case motor_IN_P2:
11319 /* Inport: '<Root>/Encode_Sp' */
11320 /* Inport: '<Root>/Down_Limit' */
11321 /* During 'P2': '<S1>:67' */
11322 if (motor_U.Encode_Sp < 0) {
11323 saturatedUnaryMinus = motor_U.Encode_Sp;
11324 if (saturatedUnaryMinus <= MIN_int32_T) {
11325 saturatedUnaryMinus = MAX_int32_T;
11326 } else {
11327 saturatedUnaryMinus = -saturatedUnaryMinus;
11328 }
11329 } else {
11330 saturatedUnaryMinus = motor_U.Encode_Sp;
11331 }
11332
11333 if ((motor_DWork.temporalCounter_i1 >= (uint32_T)(3.0 / motor_DWork.Ts)) &&
11334 ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Sp_Min)) {
11335 /* Transition: '<S1>:491' */
11336 /* 等待3S */
11337 /* Transition: '<S1>:489' */
11338 /* 判断电机是否到达下机械限位
11339 故障(卡死) */
11340 motor_DWork.is_xhzy = motor_IN_P5;
11341
11342 /* Entry 'P5': '<S1>:64' */
11343 motor_Y.Flag_Down_GZ_limit = 0U;
11344
11345 /* 下限位开关故障
11346 du:PWMOUT=PWM_Value_Up;%电机上行
11347 on after(4/Ts,tick):Flag_Down_GZ_limit=0;%下限位开关故障
11348 Motor_En=1;
11349 PWMOUT=PWM_Value_Mid;
11350 4S之后停机 */
11351 } else if (motor_U.Down_Limit == 0) {
11352 /* Transition: '<S1>:492' */
11353 /* 下限位开关低电平有效 */
11354 motor_DWork.is_xhzy = motor_IN_P4;
11355
11356 /* Entry 'P4': '<S1>:72' */
11357 motor_DWork.chu_jd -= 0.003;
11358
11359 /* Inport: '<Root>/JD_In' */
11360 /* 电机下行; */
11361 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
11362 motor_B.JD_In = motor_U.JD_In;
11363 tmp_0 = motor_DWork.Encode_Pos0;
11364 if (tmp_0 < 2.147483648E+9) {
11365 if (tmp_0 >= -2.147483648E+9) {
11366 saturatedUnaryMinus = (int32_T)tmp_0;
11367 } else {
11368 saturatedUnaryMinus = MIN_int32_T;
11369 }
11370 } else {
11371 saturatedUnaryMinus = MAX_int32_T;
11372 }
11373
11374 motor_B.Encode_Pos_c = saturatedUnaryMinus;
11375
11376 /* Inport: '<Root>/Encode_Sp' */
11377 motor_B.Encode_Sp = motor_U.Encode_Sp;
11378
11379 /* Inport: '<Root>/System_Order' */
11380 motor_B.Slect_port = motor_U.System_Order;
11381
11382 /* Inport: '<Root>/SGWY_In' */
11383 motor_B.SGWY_In = motor_U.SGWY_In;
11384
11385 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
11386 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
11387 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
11388 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
11389 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
11390 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
11391 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
11392 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
11393 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
11394 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
11395 &motor_DWork.chu_jd);
11396
11397 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
11398 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
11399 if (tmp_0 < 65536.0) {
11400 if (tmp_0 >= 0.0) {
11401 tmp = (uint16_T)tmp_0;
11402 } else {
11403 tmp = 0U;
11404 }
11405 } else {
11406 tmp = MAX_uint16_T;
11407 }
11408
11409 motor_Y.PWMOUT = tmp;
11410 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
11411 if (tmp_0 < 2.147483648E+9) {
11412 if (tmp_0 >= -2.147483648E+9) {
11413 saturatedUnaryMinus = (int32_T)tmp_0;
11414 } else {
11415 saturatedUnaryMinus = MIN_int32_T;
11416 }
11417 } else {
11418 saturatedUnaryMinus = MAX_int32_T;
11419 }
11420
11421 motor_Y.JD_Out = saturatedUnaryMinus;
11422 tmp_0 = motor_B.Motor_XHHY.KP_e;
11423 if (tmp_0 < 2.147483648E+9) {
11424 if (tmp_0 >= -2.147483648E+9) {
11425 saturatedUnaryMinus = (int32_T)tmp_0;
11426 } else {
11427 saturatedUnaryMinus = MIN_int32_T;
11428 }
11429 } else {
11430 saturatedUnaryMinus = MAX_int32_T;
11431 }
11432
11433 motor_Y.JD_Error = saturatedUnaryMinus;
11434 } else {
11435 motor_DWork.chu_jd -= 0.01;
11436
11437 /* Inport: '<Root>/JD_In' */
11438 /* 电机下行; */
11439 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
11440 motor_B.JD_In = motor_U.JD_In;
11441 tmp_0 = motor_DWork.Encode_Pos0;
11442 if (tmp_0 < 2.147483648E+9) {
11443 if (tmp_0 >= -2.147483648E+9) {
11444 saturatedUnaryMinus = (int32_T)tmp_0;
11445 } else {
11446 saturatedUnaryMinus = MIN_int32_T;
11447 }
11448 } else {
11449 saturatedUnaryMinus = MAX_int32_T;
11450 }
11451
11452 motor_B.Encode_Pos_c = saturatedUnaryMinus;
11453
11454 /* Inport: '<Root>/Encode_Sp' */
11455 motor_B.Encode_Sp = motor_U.Encode_Sp;
11456
11457 /* Inport: '<Root>/System_Order' */
11458 motor_B.Slect_port = motor_U.System_Order;
11459
11460 /* Inport: '<Root>/SGWY_In' */
11461 motor_B.SGWY_In = motor_U.SGWY_In;
11462
11463 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
11464 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
11465 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
11466 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
11467 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
11468 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
11469 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
11470 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
11471 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
11472 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
11473 &motor_DWork.chu_jd);
11474
11475 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
11476 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
11477 if (tmp_0 < 65536.0) {
11478 if (tmp_0 >= 0.0) {
11479 tmp = (uint16_T)tmp_0;
11480 } else {
11481 tmp = 0U;
11482 }
11483 } else {
11484 tmp = MAX_uint16_T;
11485 }
11486
11487 motor_Y.PWMOUT = tmp;
11488 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
11489 if (tmp_0 < 2.147483648E+9) {
11490 if (tmp_0 >= -2.147483648E+9) {
11491 saturatedUnaryMinus = (int32_T)tmp_0;
11492 } else {
11493 saturatedUnaryMinus = MIN_int32_T;
11494 }
11495 } else {
11496 saturatedUnaryMinus = MAX_int32_T;
11497 }
11498
11499 motor_Y.JD_Out = saturatedUnaryMinus;
11500 tmp_0 = motor_B.Motor_XHHY.KP_e;
11501 if (tmp_0 < 2.147483648E+9) {
11502 if (tmp_0 >= -2.147483648E+9) {
11503 saturatedUnaryMinus = (int32_T)tmp_0;
11504 } else {
11505 saturatedUnaryMinus = MIN_int32_T;
11506 }
11507 } else {
11508 saturatedUnaryMinus = MAX_int32_T;
11509 }
11510
11511 motor_Y.JD_Error = saturatedUnaryMinus;
11512
11513 /* 电机下行 */
11514 }
11515
11516 /* End of Inport: '<Root>/Down_Limit' */
11517 break;
11518
11519 case motor_IN_P3:
11520 /* During 'P3': '<S1>:127' */
11521 /* Transition: '<S1>:1034' */
11522 motor_DWork.is_xhzy = motor_IN_P2;
11523 motor_DWork.temporalCounter_i1 = 0U;
11524
11525 /* Entry 'P2': '<S1>:67' */
11526 motor_DWork.chu_jd -= 0.01;
11527
11528 /* Inport: '<Root>/JD_In' */
11529 /* 电机下行; */
11530 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
11531 motor_B.JD_In = motor_U.JD_In;
11532 tmp_0 = motor_DWork.Encode_Pos0;
11533 if (tmp_0 < 2.147483648E+9) {
11534 if (tmp_0 >= -2.147483648E+9) {
11535 saturatedUnaryMinus = (int32_T)tmp_0;
11536 } else {
11537 saturatedUnaryMinus = MIN_int32_T;
11538 }
11539 } else {
11540 saturatedUnaryMinus = MAX_int32_T;
11541 }
11542
11543 motor_B.Encode_Pos_c = saturatedUnaryMinus;
11544
11545 /* Inport: '<Root>/Encode_Sp' */
11546 motor_B.Encode_Sp = motor_U.Encode_Sp;
11547
11548 /* Inport: '<Root>/System_Order' */
11549 motor_B.Slect_port = motor_U.System_Order;
11550
11551 /* Inport: '<Root>/SGWY_In' */
11552 motor_B.SGWY_In = motor_U.SGWY_In;
11553
11554 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
11555 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
11556 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
11557 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
11558 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
11559 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
11560 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
11561 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
11562 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
11563 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
11564 &motor_DWork.chu_jd);
11565
11566 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
11567 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
11568 if (tmp_0 < 65536.0) {
11569 if (tmp_0 >= 0.0) {
11570 tmp = (uint16_T)tmp_0;
11571 } else {
11572 tmp = 0U;
11573 }
11574 } else {
11575 tmp = MAX_uint16_T;
11576 }
11577
11578 motor_Y.PWMOUT = tmp;
11579 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
11580 if (tmp_0 < 2.147483648E+9) {
11581 if (tmp_0 >= -2.147483648E+9) {
11582 saturatedUnaryMinus = (int32_T)tmp_0;
11583 } else {
11584 saturatedUnaryMinus = MIN_int32_T;
11585 }
11586 } else {
11587 saturatedUnaryMinus = MAX_int32_T;
11588 }
11589
11590 motor_Y.JD_Out = saturatedUnaryMinus;
11591 tmp_0 = motor_B.Motor_XHHY.KP_e;
11592 if (tmp_0 < 2.147483648E+9) {
11593 if (tmp_0 >= -2.147483648E+9) {
11594 saturatedUnaryMinus = (int32_T)tmp_0;
11595 } else {
11596 saturatedUnaryMinus = MIN_int32_T;
11597 }
11598 } else {
11599 saturatedUnaryMinus = MAX_int32_T;
11600 }
11601
11602 motor_Y.JD_Error = saturatedUnaryMinus;
11603 break;
11604
11605 case motor_IN_P4:
11606 /* Inport: '<Root>/Encode_Sp' */
11607 /* During 'P4': '<S1>:72' */
11608 if (motor_U.Encode_Sp < 0) {
11609 saturatedUnaryMinus = motor_U.Encode_Sp;
11610 if (saturatedUnaryMinus <= MIN_int32_T) {
11611 saturatedUnaryMinus = MAX_int32_T;
11612 } else {
11613 saturatedUnaryMinus = -saturatedUnaryMinus;
11614 }
11615 } else {
11616 saturatedUnaryMinus = motor_U.Encode_Sp;
11617 }
11618
11619 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Sp_Min) {
11620 /* Transition: '<S1>:493' */
11621 /* 判断电机是否到达下机械限位 */
11622 motor_DWork.is_xhzy = motor_IN_P10;
11623 motor_DWork.temporalCounter_i1 = 0U;
11624
11625 /* Entry 'P10': '<S1>:63' */
11626 motor_DWork.chu_jd -= 0.003;
11627
11628 /* Inport: '<Root>/JD_In' */
11629 /* 电机下行; */
11630 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
11631 motor_B.JD_In = motor_U.JD_In;
11632 tmp_0 = motor_DWork.Encode_Pos0;
11633 if (tmp_0 < 2.147483648E+9) {
11634 if (tmp_0 >= -2.147483648E+9) {
11635 saturatedUnaryMinus = (int32_T)tmp_0;
11636 } else {
11637 saturatedUnaryMinus = MIN_int32_T;
11638 }
11639 } else {
11640 saturatedUnaryMinus = MAX_int32_T;
11641 }
11642
11643 motor_B.Encode_Pos_c = saturatedUnaryMinus;
11644
11645 /* Inport: '<Root>/Encode_Sp' */
11646 motor_B.Encode_Sp = motor_U.Encode_Sp;
11647
11648 /* Inport: '<Root>/System_Order' */
11649 motor_B.Slect_port = motor_U.System_Order;
11650
11651 /* Inport: '<Root>/SGWY_In' */
11652 motor_B.SGWY_In = motor_U.SGWY_In;
11653
11654 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
11655 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
11656 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
11657 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
11658 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
11659 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
11660 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
11661 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
11662 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
11663 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
11664 &motor_DWork.chu_jd);
11665
11666 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
11667 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
11668 if (tmp_0 < 65536.0) {
11669 if (tmp_0 >= 0.0) {
11670 tmp = (uint16_T)tmp_0;
11671 } else {
11672 tmp = 0U;
11673 }
11674 } else {
11675 tmp = MAX_uint16_T;
11676 }
11677
11678 motor_Y.PWMOUT = tmp;
11679 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
11680 if (tmp_0 < 2.147483648E+9) {
11681 if (tmp_0 >= -2.147483648E+9) {
11682 saturatedUnaryMinus = (int32_T)tmp_0;
11683 } else {
11684 saturatedUnaryMinus = MIN_int32_T;
11685 }
11686 } else {
11687 saturatedUnaryMinus = MAX_int32_T;
11688 }
11689
11690 motor_Y.JD_Out = saturatedUnaryMinus;
11691 tmp_0 = motor_B.Motor_XHHY.KP_e;
11692 if (tmp_0 < 2.147483648E+9) {
11693 if (tmp_0 >= -2.147483648E+9) {
11694 saturatedUnaryMinus = (int32_T)tmp_0;
11695 } else {
11696 saturatedUnaryMinus = MIN_int32_T;
11697 }
11698 } else {
11699 saturatedUnaryMinus = MAX_int32_T;
11700 }
11701
11702 motor_Y.JD_Error = saturatedUnaryMinus;
11703 } else {
11704 motor_DWork.chu_jd -= 0.003;
11705
11706 /* Inport: '<Root>/JD_In' */
11707 /* 电机下行; */
11708 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
11709 motor_B.JD_In = motor_U.JD_In;
11710 tmp_0 = motor_DWork.Encode_Pos0;
11711 if (tmp_0 < 2.147483648E+9) {
11712 if (tmp_0 >= -2.147483648E+9) {
11713 saturatedUnaryMinus = (int32_T)tmp_0;
11714 } else {
11715 saturatedUnaryMinus = MIN_int32_T;
11716 }
11717 } else {
11718 saturatedUnaryMinus = MAX_int32_T;
11719 }
11720
11721 motor_B.Encode_Pos_c = saturatedUnaryMinus;
11722
11723 /* Inport: '<Root>/Encode_Sp' */
11724 motor_B.Encode_Sp = motor_U.Encode_Sp;
11725
11726 /* Inport: '<Root>/System_Order' */
11727 motor_B.Slect_port = motor_U.System_Order;
11728
11729 /* Inport: '<Root>/SGWY_In' */
11730 motor_B.SGWY_In = motor_U.SGWY_In;
11731
11732 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
11733 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
11734 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
11735 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
11736 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
11737 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
11738 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
11739 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
11740 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
11741 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
11742 &motor_DWork.chu_jd);
11743
11744 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
11745 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
11746 if (tmp_0 < 65536.0) {
11747 if (tmp_0 >= 0.0) {
11748 tmp = (uint16_T)tmp_0;
11749 } else {
11750 tmp = 0U;
11751 }
11752 } else {
11753 tmp = MAX_uint16_T;
11754 }
11755
11756 motor_Y.PWMOUT = tmp;
11757 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
11758 if (tmp_0 < 2.147483648E+9) {
11759 if (tmp_0 >= -2.147483648E+9) {
11760 saturatedUnaryMinus = (int32_T)tmp_0;
11761 } else {
11762 saturatedUnaryMinus = MIN_int32_T;
11763 }
11764 } else {
11765 saturatedUnaryMinus = MAX_int32_T;
11766 }
11767
11768 motor_Y.JD_Out = saturatedUnaryMinus;
11769 tmp_0 = motor_B.Motor_XHHY.KP_e;
11770 if (tmp_0 < 2.147483648E+9) {
11771 if (tmp_0 >= -2.147483648E+9) {
11772 saturatedUnaryMinus = (int32_T)tmp_0;
11773 } else {
11774 saturatedUnaryMinus = MIN_int32_T;
11775 }
11776 } else {
11777 saturatedUnaryMinus = MAX_int32_T;
11778 }
11779
11780 motor_Y.JD_Error = saturatedUnaryMinus;
11781
11782 /* 电机下行
11783 电机触碰到机械限位 */
11784 }
11785 break;
11786
11787 case motor_IN_P5:
11788 /* During 'P5': '<S1>:64' */
11789 /* Transition: '<S1>:1035' */
11790 motor_DWork.is_xhzy = motor_IN_P10;
11791 motor_DWork.temporalCounter_i1 = 0U;
11792
11793 /* Entry 'P10': '<S1>:63' */
11794 motor_DWork.chu_jd -= 0.003;
11795
11796 /* Inport: '<Root>/JD_In' */
11797 /* 电机下行; */
11798 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
11799 motor_B.JD_In = motor_U.JD_In;
11800 tmp_0 = motor_DWork.Encode_Pos0;
11801 if (tmp_0 < 2.147483648E+9) {
11802 if (tmp_0 >= -2.147483648E+9) {
11803 saturatedUnaryMinus = (int32_T)tmp_0;
11804 } else {
11805 saturatedUnaryMinus = MIN_int32_T;
11806 }
11807 } else {
11808 saturatedUnaryMinus = MAX_int32_T;
11809 }
11810
11811 motor_B.Encode_Pos_c = saturatedUnaryMinus;
11812
11813 /* Inport: '<Root>/Encode_Sp' */
11814 motor_B.Encode_Sp = motor_U.Encode_Sp;
11815
11816 /* Inport: '<Root>/System_Order' */
11817 motor_B.Slect_port = motor_U.System_Order;
11818
11819 /* Inport: '<Root>/SGWY_In' */
11820 motor_B.SGWY_In = motor_U.SGWY_In;
11821
11822 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
11823 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
11824 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
11825 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
11826 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
11827 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
11828 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
11829 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
11830 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
11831 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
11832 &motor_DWork.chu_jd);
11833
11834 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
11835 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
11836 if (tmp_0 < 65536.0) {
11837 if (tmp_0 >= 0.0) {
11838 tmp = (uint16_T)tmp_0;
11839 } else {
11840 tmp = 0U;
11841 }
11842 } else {
11843 tmp = MAX_uint16_T;
11844 }
11845
11846 motor_Y.PWMOUT = tmp;
11847 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
11848 if (tmp_0 < 2.147483648E+9) {
11849 if (tmp_0 >= -2.147483648E+9) {
11850 saturatedUnaryMinus = (int32_T)tmp_0;
11851 } else {
11852 saturatedUnaryMinus = MIN_int32_T;
11853 }
11854 } else {
11855 saturatedUnaryMinus = MAX_int32_T;
11856 }
11857
11858 motor_Y.JD_Out = saturatedUnaryMinus;
11859 tmp_0 = motor_B.Motor_XHHY.KP_e;
11860 if (tmp_0 < 2.147483648E+9) {
11861 if (tmp_0 >= -2.147483648E+9) {
11862 saturatedUnaryMinus = (int32_T)tmp_0;
11863 } else {
11864 saturatedUnaryMinus = MIN_int32_T;
11865 }
11866 } else {
11867 saturatedUnaryMinus = MAX_int32_T;
11868 }
11869
11870 motor_Y.JD_Error = saturatedUnaryMinus;
11871 break;
11872
11873 case motor_IN_P8:
11874 /* During 'P8': '<S1>:32' */
11875 break;
11876
11877 case motor_IN_p11:
11878 /* During 'p11': '<S1>:126' */
11879 if (fabs(motor_DWork.chu_jd) <= 0.02) {
11880 /* Transition: '<S1>:496' */
11881 motor_DWork.is_xhzy = motor_IN_p12;
11882 motor_DWork.temporalCounter_i1 = 0U;
11883
11884 /* Entry 'p12': '<S1>:76' */
11885 motor_Y.Motor_En = true;
11886
11887 /* 关电机 */
11888 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
11889 } else {
11890 motor_DWork.chu_jd += 0.01;
11891
11892 /* Inport: '<Root>/JD_In' */
11893 /* 电机上行回零点 */
11894 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
11895 motor_B.JD_In = motor_U.JD_In;
11896 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
11897
11898 /* Inport: '<Root>/Encode_Sp' */
11899 motor_B.Encode_Sp = motor_U.Encode_Sp;
11900
11901 /* Inport: '<Root>/System_Order' */
11902 motor_B.Slect_port = motor_U.System_Order;
11903
11904 /* Inport: '<Root>/SGWY_In' */
11905 motor_B.SGWY_In = motor_U.SGWY_In;
11906
11907 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
11908 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
11909 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
11910 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
11911 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
11912 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
11913 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
11914 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
11915 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
11916 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
11917 &motor_DWork.chu_jd);
11918
11919 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
11920 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
11921 if (tmp_0 < 65536.0) {
11922 if (tmp_0 >= 0.0) {
11923 tmp = (uint16_T)tmp_0;
11924 } else {
11925 tmp = 0U;
11926 }
11927 } else {
11928 tmp = MAX_uint16_T;
11929 }
11930
11931 motor_Y.PWMOUT = tmp;
11932 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
11933 if (tmp_0 < 2.147483648E+9) {
11934 if (tmp_0 >= -2.147483648E+9) {
11935 saturatedUnaryMinus = (int32_T)tmp_0;
11936 } else {
11937 saturatedUnaryMinus = MIN_int32_T;
11938 }
11939 } else {
11940 saturatedUnaryMinus = MAX_int32_T;
11941 }
11942
11943 motor_Y.JD_Out = saturatedUnaryMinus;
11944 tmp_0 = motor_B.Motor_XHHY.KP_e;
11945 if (tmp_0 < 2.147483648E+9) {
11946 if (tmp_0 >= -2.147483648E+9) {
11947 saturatedUnaryMinus = (int32_T)tmp_0;
11948 } else {
11949 saturatedUnaryMinus = MIN_int32_T;
11950 }
11951 } else {
11952 saturatedUnaryMinus = MAX_int32_T;
11953 }
11954
11955 motor_Y.JD_Error = saturatedUnaryMinus;
11956 }
11957 break;
11958
11959 default:
11960 /* During 'p12': '<S1>:76' */
11961 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.2 / motor_DWork.Ts)) {
11962 /* Transition: '<S1>:494' */
11963 /* Transition: '<S1>:406' */
11964 motor_DWork.is_xhzy = motor_IN_NO_ACTIVE_CHILD;
11965 motor_DWork.is_Initialize = motor_IN_Check_Reset;
11966
11967 /* Entry 'Check_Reset': '<S1>:71' */
11968 motor_Y.Ini_Result = motor_Init_Result_Check(motor_Y.Flag_Cur,
11969 motor_Y.Flag_Temp_Up, (real_T)motor_Y.Flag_Temp_Down);
11970 motor_Y.DCZD = true;
11971 }
11972 break;
11973 }
11974 break;
11975 }
11976
11977 if (guard2) {
11978 /* 开启制动 */
11979 motor_Y.Ini_Result = motor_Init_Result_Check(motor_Y.Flag_Cur,
11980 motor_Y.Flag_Temp_Up, (real_T)motor_Y.Flag_Temp_Down);
11981 }
11982
11983 if (guard1) {
11984 /* Inport: '<Root>/Up_Limit' */
11985 if (motor_U.Up_Limit == 0) {
11986 /* Transition: '<S1>:488' */
11987 /* 上限位开关低电平有效 */
11988 motor_DWork.is_xhzy = motor_IN_P11;
11989 motor_DWork.temporalCounter_i1 = 0U;
11990
11991 /* Entry 'P11': '<S1>:1170' */
11992 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
11993 } else {
11994 motor_DWork.chu_jd += 0.01;
11995
11996 /* Inport: '<Root>/JD_In' */
11997 /* 电机上行; */
11998 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
11999 motor_B.JD_In = motor_U.JD_In;
12000 tmp_0 = motor_DWork.Encode_Pos0;
12001 if (tmp_0 < 2.147483648E+9) {
12002 if (tmp_0 >= -2.147483648E+9) {
12003 saturatedUnaryMinus = (int32_T)tmp_0;
12004 } else {
12005 saturatedUnaryMinus = MIN_int32_T;
12006 }
12007 } else {
12008 saturatedUnaryMinus = MAX_int32_T;
12009 }
12010
12011 motor_B.Encode_Pos_c = saturatedUnaryMinus;
12012
12013 /* Inport: '<Root>/Encode_Sp' */
12014 motor_B.Encode_Sp = motor_U.Encode_Sp;
12015
12016 /* Inport: '<Root>/System_Order' */
12017 motor_B.Slect_port = motor_U.System_Order;
12018
12019 /* Inport: '<Root>/SGWY_In' */
12020 motor_B.SGWY_In = motor_U.SGWY_In;
12021
12022 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
12023 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
12024 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
12025 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
12026 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
12027 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
12028 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
12029 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
12030 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
12031 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
12032 &motor_DWork.chu_jd);
12033
12034 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
12035 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
12036 if (tmp_0 < 65536.0) {
12037 if (tmp_0 >= 0.0) {
12038 tmp = (uint16_T)tmp_0;
12039 } else {
12040 tmp = 0U;
12041 }
12042 } else {
12043 tmp = MAX_uint16_T;
12044 }
12045
12046 motor_Y.PWMOUT = tmp;
12047 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
12048 if (tmp_0 < 2.147483648E+9) {
12049 if (tmp_0 >= -2.147483648E+9) {
12050 saturatedUnaryMinus = (int32_T)tmp_0;
12051 } else {
12052 saturatedUnaryMinus = MIN_int32_T;
12053 }
12054 } else {
12055 saturatedUnaryMinus = MAX_int32_T;
12056 }
12057
12058 motor_Y.JD_Out = saturatedUnaryMinus;
12059 tmp_0 = motor_B.Motor_XHHY.KP_e;
12060 if (tmp_0 < 2.147483648E+9) {
12061 if (tmp_0 >= -2.147483648E+9) {
12062 saturatedUnaryMinus = (int32_T)tmp_0;
12063 } else {
12064 saturatedUnaryMinus = MIN_int32_T;
12065 }
12066 } else {
12067 saturatedUnaryMinus = MAX_int32_T;
12068 }
12069
12070 motor_Y.JD_Error = saturatedUnaryMinus;
12071
12072 /* 电机上行 */
12073 }
12074
12075 /* End of Inport: '<Root>/Up_Limit' */
12076 }
12077}
12078
12079/* Function for Chart: '<Root>/Chart' */
12080static void motor_enter_atomic_XHZY_Run(void)
12081{
12082 /* Entry 'XHZY_Run': '<S1>:150' */
12083 motor_DWork.KG_En = 0U;
12084 motor_DWork.KG_JD = 1U;
12085 motor_DWork.KG_YJ = 1U;
12086 motor_DWork.EN_extern = 0U;
12087 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
12088 motor_Y.Open_Result = 1U;
12089 motor_DWork.Forword = 0U;
12090 motor_DWork.P_KP = 380.0;
12091 motor_DWork.V_KP = 1.0;
12092 motor_DWork.V_KI = 10.0;
12093 motor_DWork.Runing_stable = 1U;
12094}
12095
12096/* Function for Chart: '<Root>/Chart' */
12097static void motor_enter_atomic_XHZY_Run1(void)
12098{
12099 /* Entry 'XHZY_Run1': '<S1>:1182' */
12100 motor_DWork.KG_En = 0U;
12101 motor_DWork.KG_JD = 1U;
12102 motor_DWork.KG_YJ = 1U;
12103 motor_DWork.EN_extern = 1U;
12104 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
12105 motor_Y.Open_Result = 1U;
12106 motor_DWork.Forword = 0U;
12107 motor_DWork.P_KP = 150.0;
12108 motor_DWork.V_KP = 0.5;
12109 motor_DWork.V_KI = 8.0;
12110 motor_DWork.Runing_stable = 1U;
12111}
12112
12113/* Function for Chart: '<Root>/Chart' */
12114static void motor_exit_internal_Normal_Mode(void)
12115{
12116 /* Exit Internal 'Normal_Mode': '<S1>:13' */
12117 if (motor_DWork.is_Normal_Mode == motor_IN_p1_a) {
12118 /* Exit Internal 'p1': '<S1>:14' */
12119 /* Exit Internal 'Enc_GZ': '<S1>:15' */
12120 /* Exit Internal 'Enc2': '<S1>:16' */
12121 motor_DWork.is_Enc2 = motor_IN_NO_ACTIVE_CHILD;
12122 motor_DWork.is_active_Enc2 = 0U;
12123
12124 /* Exit Internal 'Enc1': '<S1>:20' */
12125 motor_DWork.is_Enc1 = motor_IN_NO_ACTIVE_CHILD;
12126 motor_DWork.is_active_Enc1 = 0U;
12127 motor_DWork.is_active_Enc_GZ = 0U;
12128
12129 /* Exit Internal 'RUN': '<S1>:23' */
12130 /* Exit Internal 'Error_Check': '<S1>:1294' */
12131 /* Exit Internal 'Error_Check': '<S1>:1306' */
12132 motor_DWork.is_Error_Check_g = motor_IN_NO_ACTIVE_CHILD;
12133 motor_DWork.is_Error_Check = motor_IN_NO_ACTIVE_CHILD;
12134
12135 /* Exit 'Error_Check': '<S1>:1294' */
12136 motor_Y.Flag_AngleError = 1U;
12137 motor_DWork.is_active_Error_Check = 0U;
12138
12139 /* Exit Internal 'Algorithm': '<S1>:165' */
12140 switch (motor_DWork.is_Algorithm_l) {
12141 case motor_IN_HY:
12142 /* Exit Internal 'HY': '<S1>:167' */
12143 switch (motor_DWork.is_HY) {
12144 case motor_IN_Sleeping_protect_2:
12145 /* Exit 'Sleeping_protect_2': '<S1>:166' */
12146 /* 关电磁制动 */
12147 motor_Y.DCZD = false;
12148 motor_DWork.is_HY = motor_IN_NO_ACTIVE_CHILD;
12149 break;
12150
12151 case motor_IN_XHHY_Run:
12152 /* Exit 'XHHY_Run': '<S1>:170' */
12153 motor_DWork.Runing_stable = 0U;
12154 motor_DWork.is_HY = motor_IN_NO_ACTIVE_CHILD;
12155 break;
12156
12157 case motor_IN_XHHY_Run1:
12158 /* Exit 'XHHY_Run1': '<S1>:1232' */
12159 motor_DWork.Runing_stable = 0U;
12160 motor_DWork.is_HY = motor_IN_NO_ACTIVE_CHILD;
12161 break;
12162
12163 default:
12164 motor_DWork.is_HY = motor_IN_NO_ACTIVE_CHILD;
12165 break;
12166 }
12167
12168 /* Exit 'HY': '<S1>:167' */
12169 motor_DWork.P_KP = 1200.0;
12170 motor_DWork.V_KP = 0.08;
12171 motor_DWork.V_KI = 0.8;
12172 motor_DWork.is_Algorithm_l = motor_IN_NO_ACTIVE_CHILD;
12173 break;
12174
12175 case motor_IN_XHHY:
12176 /* Exit Internal 'XHHY': '<S1>:149' */
12177 switch (motor_DWork.is_XHHY) {
12178 case motor_IN_Sleeping_protect_2:
12179 /* Exit 'Sleeping_protect_2': '<S1>:174' */
12180 /* 关电磁制动 */
12181 motor_Y.DCZD = false;
12182 motor_DWork.is_XHHY = motor_IN_NO_ACTIVE_CHILD;
12183 break;
12184
12185 case motor_IN_XHHY_Run:
12186 /* Exit 'XHHY_Run': '<S1>:364' */
12187 motor_DWork.Runing_stable = 0U;
12188 motor_DWork.is_XHHY = motor_IN_NO_ACTIVE_CHILD;
12189 break;
12190
12191 case motor_IN_XHHY_Run1:
12192 /* Exit 'XHHY_Run1': '<S1>:1198' */
12193 motor_DWork.Runing_stable = 0U;
12194 motor_DWork.is_XHHY = motor_IN_NO_ACTIVE_CHILD;
12195 break;
12196
12197 default:
12198 motor_DWork.is_XHHY = motor_IN_NO_ACTIVE_CHILD;
12199 break;
12200 }
12201
12202 /* Exit 'XHHY': '<S1>:149' */
12203 motor_DWork.P_KP = 330.0;
12204 motor_DWork.V_KP = 1.0;
12205 motor_DWork.V_KI = 10.0;
12206 motor_DWork.is_Algorithm_l = motor_IN_NO_ACTIVE_CHILD;
12207 break;
12208
12209 case motor_IN_XHZY:
12210 /* Exit Internal 'XHZY': '<S1>:172' */
12211 switch (motor_DWork.is_XHZY) {
12212 case motor_IN_Sleeping_protect_2:
12213 /* Exit 'Sleeping_protect_2': '<S1>:236' */
12214 /* 关电磁制动 */
12215 motor_Y.DCZD = false;
12216 motor_DWork.is_XHZY = motor_IN_NO_ACTIVE_CHILD;
12217 break;
12218
12219 case motor_IN_XHZY_Run:
12220 /* Exit 'XHZY_Run': '<S1>:150' */
12221 motor_DWork.Runing_stable = 0U;
12222 motor_DWork.is_XHZY = motor_IN_NO_ACTIVE_CHILD;
12223 break;
12224
12225 case motor_IN_XHZY_Run1:
12226 /* Exit 'XHZY_Run1': '<S1>:1182' */
12227 motor_DWork.Runing_stable = 0U;
12228 motor_DWork.is_XHZY = motor_IN_NO_ACTIVE_CHILD;
12229 break;
12230
12231 default:
12232 motor_DWork.is_XHZY = motor_IN_NO_ACTIVE_CHILD;
12233 break;
12234 }
12235
12236 /* Exit 'XHZY': '<S1>:172' */
12237 motor_DWork.P_KP = 380.0;
12238 motor_DWork.V_KP = 1.0;
12239 motor_DWork.V_KI = 10.0;
12240 motor_DWork.Forword = 1U;
12241 motor_DWork.is_Algorithm_l = motor_IN_NO_ACTIVE_CHILD;
12242 break;
12243
12244 default:
12245 motor_DWork.is_Algorithm_l = motor_IN_NO_ACTIVE_CHILD;
12246 break;
12247 }
12248
12249 /* Exit 'Algorithm': '<S1>:165' */
12250 motor_DWork.EN_extern = 0U;
12251 motor_DWork.is_active_Algorithm = 0U;
12252
12253 /* Exit Internal 'Limit_Check': '<S1>:176' */
12254 motor_DWork.is_Limit_Check = motor_IN_NO_ACTIVE_CHILD;
12255 motor_DWork.is_active_Limit_Check = 0U;
12256
12257 /* Exit Internal 'GXDY_SJ_BJ': '<S1>:154' */
12258 motor_DWork.is_GXDY_SJ_BJ = motor_IN_NO_ACTIVE_CHILD;
12259 motor_DWork.is_active_GXDY_SJ_BJ = 0U;
12260
12261 /* Exit Internal 'GXDY_COM_GZ': '<S1>:162' */
12262 motor_DWork.is_GXDY_COM_GZ = motor_IN_NO_ACTIVE_CHILD;
12263 motor_DWork.is_active_GXDY_COM_GZ = 0U;
12264 motor_DWork.is_active_RUN = 0U;
12265 motor_DWork.is_Normal_Mode = motor_IN_NO_ACTIVE_CHILD;
12266 } else {
12267 motor_DWork.is_Normal_Mode = motor_IN_NO_ACTIVE_CHILD;
12268 }
12269}
12270
12271/* Function for Chart: '<Root>/Chart' */
12272static void motor_HY(void)
12273{
12274 int32_T tmp;
12275 real_T tmp_0;
12276
12277 /* Inport: '<Root>/System_Order' */
12278 /* During 'HY': '<S1>:167' */
12279 if (motor_U.System_Order == 3) {
12280 /* Transition: '<S1>:558' */
12281 /* Transition: '<S1>:557' */
12282 motor_exit_internal_Normal_Mode();
12283 motor_DWork.is_M_Run = motor_IN_Close;
12284 motor_DWork.is_Close = motor_IN_Close_Wait;
12285 motor_DWork.temporalCounter_i1 = 0U;
12286
12287 /* Entry 'Close_Wait': '<S1>:241' */
12288 motor_Y.Open_Result = 4U;
12289
12290 /* 正在关机 */
12291 motor_Y.DCZD = false;
12292
12293 /* 解除制动 */
12294 motor_Y.Motor_En = false;
12295
12296 /* 电机使能 */
12297 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
12298 motor_DWork.EN_extern = 0U;
12299 } else {
12300 switch (motor_DWork.is_HY) {
12301 case motor_IN_Sleeping_protect_1:
12302 /* During 'Sleeping_protect_1': '<S1>:233' */
12303 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
12304 /* Transition: '<S1>:563' */
12305 motor_DWork.is_HY = motor_IN_Sleeping_protect_2;
12306
12307 /* Entry 'Sleeping_protect_2': '<S1>:166' */
12308 motor_Y.DCZD = true;
12309
12310 /* 开电磁制动 */
12311 }
12312 break;
12313
12314 case motor_IN_Sleeping_protect_2:
12315 /* During 'Sleeping_protect_2': '<S1>:166' */
12316 if (motor_U.System_Order == 5) {
12317 /* Transition: '<S1>:566' */
12318 /* Exit 'Sleeping_protect_2': '<S1>:166' */
12319 /* 关电磁制动 */
12320 motor_Y.DCZD = false;
12321 motor_DWork.is_HY = motor_IN_Sleeping_protect_3;
12322 motor_DWork.temporalCounter_i1 = 0U;
12323
12324 /* Entry 'Sleeping_protect_3': '<S1>:232' */
12325 motor_Y.Motor_En = false;
12326
12327 /* 开电机 */
12328 }
12329 break;
12330
12331 case motor_IN_Sleeping_protect_3:
12332 /* During 'Sleeping_protect_3': '<S1>:232' */
12333 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
12334 /* Inport: '<Root>/Gyroscope_Choose' */
12335 /* Transition: '<S1>:569' */
12336 if (motor_U.Gyroscope_Choose == 0) {
12337 /* Transition: '<S1>:1236' */
12338 motor_DWork.is_HY = motor_IN_XHHY_Wait1;
12339 motor_DWork.temporalCounter_i1 = 0U;
12340
12341 /* Entry 'XHHY_Wait1': '<S1>:365' */
12342 motor_DWork.KG_JD = 1U;
12343 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
12344 motor_Y.Open_Result = 1U;
12345 motor_DWork.Rate_limit_speed = 5000.0;
12346 motor_DWork.P_KP = 1200.0;
12347 motor_DWork.V_KP = 0.08;
12348 motor_DWork.V_KI = 0.8;
12349 motor_DWork.Saturation_limit_speed = 3000.0;
12350 } else {
12351 if (motor_U.Gyroscope_Choose == 1) {
12352 /* Transition: '<S1>:1234' */
12353 motor_DWork.is_HY = motor_IN_XHHY_Wait2;
12354 motor_DWork.temporalCounter_i1 = 0U;
12355
12356 /* Entry 'XHHY_Wait2': '<S1>:1227' */
12357 motor_DWork.KG_JD = 1U;
12358 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
12359 motor_Y.Open_Result = 1U;
12360 motor_DWork.Rate_limit_speed = 5000.0;
12361 motor_DWork.P_KP = 300.0;
12362 motor_DWork.V_KP = 0.08;
12363 motor_DWork.V_KI = 0.8;
12364 motor_DWork.Saturation_limit_speed = 3000.0;
12365 }
12366 }
12367 }
12368 break;
12369
12370 case motor_IN_XHHY_Error1:
12371 /* Inport: '<Root>/GXDY_State' */
12372 /* During 'XHHY_Error1': '<S1>:1138' */
12373 if (!motor_U.GXDY_State) {
12374 /* Transition: '<S1>:1139' */
12375 motor_Y.Motor_En = false;
12376 motor_DWork.is_HY = motor_IN_XHHY_Wait;
12377 motor_DWork.temporalCounter_i1 = 0U;
12378
12379 /* Entry 'XHHY_Wait': '<S1>:178' */
12380 /* Open_Result=uint8(1);
12381 开机状态位成功 */
12382 motor_DWork.In_State = 2U;
12383 motor_Y.PWMOUT = 2500U;
12384
12385 /* during:
12386 [PWMOUT,JD_Error,JD_Out] = Motor_HYDG(JD_In,Encode_Pos,System_Order,Encode_Sp); */
12387 }
12388 break;
12389
12390 case motor_IN_XHHY_Run:
12391 /* Inport: '<Root>/Gyroscope_Choose' */
12392 /* During 'XHHY_Run': '<S1>:170' */
12393 if (motor_U.System_Order == 4) {
12394 /* Transition: '<S1>:567' */
12395 /* Exit 'XHHY_Run': '<S1>:170' */
12396 motor_DWork.Runing_stable = 0U;
12397 motor_DWork.is_HY = motor_IN_Sleeping_protect_1;
12398 motor_DWork.temporalCounter_i1 = 0U;
12399
12400 /* Entry 'Sleeping_protect_1': '<S1>:233' */
12401 motor_Y.Motor_En = true;
12402
12403 /* 关电机 */
12404 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
12405 } else if (motor_U.Gyroscope_Choose != 0) {
12406 /* Transition: '<S1>:1237' */
12407 /* Transition: '<S1>:1239' */
12408 /* Transition: '<S1>:557' */
12409 motor_exit_internal_Normal_Mode();
12410 motor_DWork.is_M_Run = motor_IN_Close;
12411 motor_DWork.is_Close = motor_IN_Close_Wait;
12412 motor_DWork.temporalCounter_i1 = 0U;
12413
12414 /* Entry 'Close_Wait': '<S1>:241' */
12415 motor_Y.Open_Result = 4U;
12416
12417 /* 正在关机 */
12418 motor_Y.DCZD = false;
12419
12420 /* 解除制动 */
12421 motor_Y.Motor_En = false;
12422
12423 /* 电机使能 */
12424 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
12425 motor_DWork.EN_extern = 0U;
12426 } else {
12427 /* Inport: '<Root>/JD_In' */
12428 /* 外部接口 */
12429 /* Simulink Function 'Motor_HYDG1': '<S1>:115' */
12430 motor_B.JD_In_n = motor_U.JD_In;
12431 motor_B.Encode_Pos_cm = motor_Y.Encode_Pos;
12432 motor_B.Slect_port_e = motor_U.System_Order;
12433
12434 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG1' */
12435 motor_Motor_HYDG1(motor_M, motor_B.JD_In_n, motor_B.Encode_Pos_cm,
12436 motor_B.Slect_port_e, &motor_B.Motor_HYDG1,
12437 &motor_DWork.Motor_HYDG1, (rtP_Motor_HYDG1_motor *)
12438 &motor_P.Motor_HYDG1, &motor_DWork.JD_HYDG,
12439 &motor_DWork.JD_HYDG_HC, &motor_DWork.KG_JD,
12440 &motor_DWork.KG_clc, &motor_DWork.P_KP,
12441 &motor_DWork.Saturation_limit_speed, &motor_DWork.V_KI,
12442 &motor_DWork.V_KP, &motor_DWork.chu_jd);
12443
12444 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG1' */
12445 motor_Y.PWMOUT = motor_B.Motor_HYDG1.DataTypeConversion3;
12446 tmp_0 = motor_B.Motor_HYDG1.KP_e;
12447 if (tmp_0 < 2.147483648E+9) {
12448 if (tmp_0 >= -2.147483648E+9) {
12449 tmp = (int32_T)tmp_0;
12450 } else {
12451 tmp = MIN_int32_T;
12452 }
12453 } else {
12454 tmp = MAX_int32_T;
12455 }
12456
12457 motor_Y.JD_Error = tmp;
12458 tmp_0 = motor_B.Motor_HYDG1.KP_JD;
12459 if (tmp_0 < 2.147483648E+9) {
12460 if (tmp_0 >= -2.147483648E+9) {
12461 tmp = (int32_T)tmp_0;
12462 } else {
12463 tmp = MIN_int32_T;
12464 }
12465 } else {
12466 tmp = MAX_int32_T;
12467 }
12468
12469 motor_Y.JD_Out = tmp;
12470 }
12471 break;
12472
12473 case motor_IN_XHHY_Run1:
12474 /* Inport: '<Root>/Gyroscope_Choose' */
12475 /* During 'XHHY_Run1': '<S1>:1232' */
12476 if (motor_U.System_Order == 4) {
12477 /* Transition: '<S1>:1233' */
12478 /* Exit 'XHHY_Run1': '<S1>:1232' */
12479 motor_DWork.Runing_stable = 0U;
12480 motor_DWork.is_HY = motor_IN_Sleeping_protect_1;
12481 motor_DWork.temporalCounter_i1 = 0U;
12482
12483 /* Entry 'Sleeping_protect_1': '<S1>:233' */
12484 motor_Y.Motor_En = true;
12485
12486 /* 关电机 */
12487 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
12488 } else if (motor_U.Gyroscope_Choose != 1) {
12489 /* Transition: '<S1>:1238' */
12490 /* Transition: '<S1>:1239' */
12491 /* Transition: '<S1>:557' */
12492 motor_exit_internal_Normal_Mode();
12493 motor_DWork.is_M_Run = motor_IN_Close;
12494 motor_DWork.is_Close = motor_IN_Close_Wait;
12495 motor_DWork.temporalCounter_i1 = 0U;
12496
12497 /* Entry 'Close_Wait': '<S1>:241' */
12498 motor_Y.Open_Result = 4U;
12499
12500 /* 正在关机 */
12501 motor_Y.DCZD = false;
12502
12503 /* 解除制动 */
12504 motor_Y.Motor_En = false;
12505
12506 /* 电机使能 */
12507 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
12508 motor_DWork.EN_extern = 0U;
12509 } else {
12510 /* Inport: '<Root>/JD_In' */
12511 /* 外部接口 */
12512 /* Simulink Function 'Waveform_Build': '<S1>:1223' */
12513 motor_B.JD_In_c = motor_U.JD_In;
12514
12515 /* Outputs for Function Call SubSystem: '<S1>/Waveform_Build' */
12516 motor_Waveform_Build(motor_M, motor_B.JD_In_c, &motor_B.Waveform_Build,
12517 &motor_DWork.Waveform_Build,
12518 (rtP_Waveform_Build_motor *)&motor_P.Waveform_Build,
12519 &motor_DWork.T, &motor_DWork.T_Count,
12520 &motor_DWork.Temp1, &motor_DWork.Temp2);
12521
12522 /* End of Outputs for SubSystem: '<S1>/Waveform_Build' */
12523
12524 /* JD_forecast = JD_In; */
12525 /* Simulink Function 'Motor_HYDG1': '<S1>:115' */
12526 motor_B.JD_In_n = motor_B.Waveform_Build.Gain2;
12527 motor_B.Encode_Pos_cm = motor_Y.Encode_Pos;
12528 motor_B.Slect_port_e = motor_U.System_Order;
12529
12530 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG1' */
12531 motor_Motor_HYDG1(motor_M, motor_B.JD_In_n, motor_B.Encode_Pos_cm,
12532 motor_B.Slect_port_e, &motor_B.Motor_HYDG1,
12533 &motor_DWork.Motor_HYDG1, (rtP_Motor_HYDG1_motor *)
12534 &motor_P.Motor_HYDG1, &motor_DWork.JD_HYDG,
12535 &motor_DWork.JD_HYDG_HC, &motor_DWork.KG_JD,
12536 &motor_DWork.KG_clc, &motor_DWork.P_KP,
12537 &motor_DWork.Saturation_limit_speed, &motor_DWork.V_KI,
12538 &motor_DWork.V_KP, &motor_DWork.chu_jd);
12539
12540 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG1' */
12541 motor_Y.PWMOUT = motor_B.Motor_HYDG1.DataTypeConversion3;
12542 tmp_0 = motor_B.Motor_HYDG1.KP_e;
12543 if (tmp_0 < 2.147483648E+9) {
12544 if (tmp_0 >= -2.147483648E+9) {
12545 tmp = (int32_T)tmp_0;
12546 } else {
12547 tmp = MIN_int32_T;
12548 }
12549 } else {
12550 tmp = MAX_int32_T;
12551 }
12552
12553 motor_Y.JD_Error = tmp;
12554 tmp_0 = motor_B.Motor_HYDG1.KP_JD;
12555 if (tmp_0 < 2.147483648E+9) {
12556 if (tmp_0 >= -2.147483648E+9) {
12557 tmp = (int32_T)tmp_0;
12558 } else {
12559 tmp = MIN_int32_T;
12560 }
12561 } else {
12562 tmp = MAX_int32_T;
12563 }
12564
12565 motor_Y.JD_Out = tmp;
12566 }
12567 break;
12568
12569 case motor_IN_XHHY_Wait:
12570 /* During 'XHHY_Wait': '<S1>:178' */
12571 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
12572 /* Inport: '<Root>/Gyroscope_Choose' */
12573 /* Transition: '<S1>:983' */
12574 if (motor_U.Gyroscope_Choose == 1) {
12575 /* Transition: '<S1>:1226' */
12576 motor_DWork.is_HY = motor_IN_XHHY_Wait2;
12577 motor_DWork.temporalCounter_i1 = 0U;
12578
12579 /* Entry 'XHHY_Wait2': '<S1>:1227' */
12580 motor_DWork.KG_JD = 1U;
12581 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
12582 motor_Y.Open_Result = 1U;
12583 motor_DWork.Rate_limit_speed = 5000.0;
12584 motor_DWork.P_KP = 300.0;
12585 motor_DWork.V_KP = 0.08;
12586 motor_DWork.V_KI = 0.8;
12587 motor_DWork.Saturation_limit_speed = 3000.0;
12588 } else {
12589 if (motor_U.Gyroscope_Choose == 0) {
12590 /* Inport: '<Root>/GXDY_State' */
12591 /* Transition: '<S1>:571' */
12592 if (motor_U.GXDY_State) {
12593 /* Transition: '<S1>:984' */
12594 motor_DWork.is_HY = motor_IN_XHHY_Error1;
12595
12596 /* Entry 'XHHY_Error1': '<S1>:1138' */
12597 motor_Y.Motor_En = true;
12598
12599 /* 关电机 */
12600 motor_Y.Open_Result = 2U;
12601 } else {
12602 /* Transition: '<S1>:1254' */
12603 /* 惯性单元拉低 */
12604 motor_DWork.is_HY = motor_IN_XHHY_Wait1;
12605 motor_DWork.temporalCounter_i1 = 0U;
12606
12607 /* Entry 'XHHY_Wait1': '<S1>:365' */
12608 motor_DWork.KG_JD = 1U;
12609 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
12610 motor_Y.Open_Result = 1U;
12611 motor_DWork.Rate_limit_speed = 5000.0;
12612 motor_DWork.P_KP = 1200.0;
12613 motor_DWork.V_KP = 0.08;
12614 motor_DWork.V_KI = 0.8;
12615 motor_DWork.Saturation_limit_speed = 3000.0;
12616 }
12617 }
12618 }
12619 }
12620 break;
12621
12622 case motor_IN_XHHY_Wait1:
12623 /* During 'XHHY_Wait1': '<S1>:365' */
12624 if (motor_DWork.Saturation_limit_speed >= 16000.0) {
12625 /* Transition: '<S1>:570' */
12626 motor_DWork.is_HY = motor_IN_XHHY_Run;
12627
12628 /* Entry 'XHHY_Run': '<S1>:170' */
12629 motor_DWork.KG_JD = 1U;
12630 motor_DWork.Rate_limit_speed = 50000.0;
12631 motor_DWork.Saturation_limit_speed = 16000.0;
12632 motor_DWork.Runing_stable = 1U;
12633 } else if (motor_DWork.temporalCounter_i1 >= 10U) {
12634 /* Transition: '<S1>:1178' */
12635 motor_DWork.is_HY = motor_IN_XHHY_Run;
12636
12637 /* Entry 'XHHY_Run': '<S1>:170' */
12638 motor_DWork.KG_JD = 1U;
12639 motor_DWork.Rate_limit_speed = 50000.0;
12640 motor_DWork.Saturation_limit_speed = 16000.0;
12641 motor_DWork.Runing_stable = 1U;
12642 } else {
12643 /* Inport: '<Root>/JD_In' */
12644 /* Simulink Function 'Motor_HYDG1': '<S1>:115' */
12645 motor_B.JD_In_n = motor_U.JD_In;
12646 motor_B.Encode_Pos_cm = motor_Y.Encode_Pos;
12647 motor_B.Slect_port_e = motor_U.System_Order;
12648
12649 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG1' */
12650 motor_Motor_HYDG1(motor_M, motor_B.JD_In_n, motor_B.Encode_Pos_cm,
12651 motor_B.Slect_port_e, &motor_B.Motor_HYDG1,
12652 &motor_DWork.Motor_HYDG1, (rtP_Motor_HYDG1_motor *)
12653 &motor_P.Motor_HYDG1, &motor_DWork.JD_HYDG,
12654 &motor_DWork.JD_HYDG_HC, &motor_DWork.KG_JD,
12655 &motor_DWork.KG_clc, &motor_DWork.P_KP,
12656 &motor_DWork.Saturation_limit_speed, &motor_DWork.V_KI,
12657 &motor_DWork.V_KP, &motor_DWork.chu_jd);
12658
12659 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG1' */
12660 motor_Y.PWMOUT = motor_B.Motor_HYDG1.DataTypeConversion3;
12661 tmp_0 = motor_B.Motor_HYDG1.KP_e;
12662 if (tmp_0 < 2.147483648E+9) {
12663 if (tmp_0 >= -2.147483648E+9) {
12664 tmp = (int32_T)tmp_0;
12665 } else {
12666 tmp = MIN_int32_T;
12667 }
12668 } else {
12669 tmp = MAX_int32_T;
12670 }
12671
12672 motor_Y.JD_Error = tmp;
12673 tmp_0 = motor_B.Motor_HYDG1.KP_JD;
12674 if (tmp_0 < 2.147483648E+9) {
12675 if (tmp_0 >= -2.147483648E+9) {
12676 tmp = (int32_T)tmp_0;
12677 } else {
12678 tmp = MIN_int32_T;
12679 }
12680 } else {
12681 tmp = MAX_int32_T;
12682 }
12683
12684 motor_Y.JD_Out = tmp;
12685
12686 /* Saturation_limit_speed=Saturation_limit_speed+12; */
12687 motor_DWork.Saturation_limit_speed = fabs((16000.0 -
12688 motor_DWork.Saturation_limit_speed) / (real_T)mul_s32_s32_s32_sat
12689 (100000, motor_Y.JD_Error)) + (motor_DWork.Saturation_limit_speed +
12690 5.0);
12691 }
12692 break;
12693
12694 default:
12695 /* During 'XHHY_Wait2': '<S1>:1227' */
12696 if (motor_DWork.Saturation_limit_speed >= 16000.0) {
12697 /* Transition: '<S1>:1231' */
12698 motor_DWork.is_HY = motor_IN_XHHY_Run1;
12699
12700 /* Entry 'XHHY_Run1': '<S1>:1232' */
12701 motor_DWork.KG_JD = 1U;
12702 motor_DWork.Rate_limit_speed = 50000.0;
12703 motor_DWork.Saturation_limit_speed = 16000.0;
12704 motor_DWork.Runing_stable = 1U;
12705 } else if (motor_DWork.temporalCounter_i1 >= 10U) {
12706 /* Transition: '<S1>:1229' */
12707 motor_DWork.is_HY = motor_IN_XHHY_Run1;
12708
12709 /* Entry 'XHHY_Run1': '<S1>:1232' */
12710 motor_DWork.KG_JD = 1U;
12711 motor_DWork.Rate_limit_speed = 50000.0;
12712 motor_DWork.Saturation_limit_speed = 16000.0;
12713 motor_DWork.Runing_stable = 1U;
12714 } else {
12715 /* Inport: '<Root>/JD_In' */
12716 /* Simulink Function 'Waveform_Build': '<S1>:1223' */
12717 motor_B.JD_In_c = motor_U.JD_In;
12718
12719 /* Outputs for Function Call SubSystem: '<S1>/Waveform_Build' */
12720 motor_Waveform_Build(motor_M, motor_B.JD_In_c, &motor_B.Waveform_Build,
12721 &motor_DWork.Waveform_Build,
12722 (rtP_Waveform_Build_motor *)&motor_P.Waveform_Build,
12723 &motor_DWork.T, &motor_DWork.T_Count,
12724 &motor_DWork.Temp1, &motor_DWork.Temp2);
12725
12726 /* End of Outputs for SubSystem: '<S1>/Waveform_Build' */
12727
12728 /* Simulink Function 'Motor_HYDG1': '<S1>:115' */
12729 motor_B.JD_In_n = motor_B.Waveform_Build.Gain2;
12730 motor_B.Encode_Pos_cm = motor_Y.Encode_Pos;
12731 motor_B.Slect_port_e = motor_U.System_Order;
12732
12733 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG1' */
12734 motor_Motor_HYDG1(motor_M, motor_B.JD_In_n, motor_B.Encode_Pos_cm,
12735 motor_B.Slect_port_e, &motor_B.Motor_HYDG1,
12736 &motor_DWork.Motor_HYDG1, (rtP_Motor_HYDG1_motor *)
12737 &motor_P.Motor_HYDG1, &motor_DWork.JD_HYDG,
12738 &motor_DWork.JD_HYDG_HC, &motor_DWork.KG_JD,
12739 &motor_DWork.KG_clc, &motor_DWork.P_KP,
12740 &motor_DWork.Saturation_limit_speed, &motor_DWork.V_KI,
12741 &motor_DWork.V_KP, &motor_DWork.chu_jd);
12742
12743 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG1' */
12744 motor_Y.PWMOUT = motor_B.Motor_HYDG1.DataTypeConversion3;
12745 tmp_0 = motor_B.Motor_HYDG1.KP_e;
12746 if (tmp_0 < 2.147483648E+9) {
12747 if (tmp_0 >= -2.147483648E+9) {
12748 tmp = (int32_T)tmp_0;
12749 } else {
12750 tmp = MIN_int32_T;
12751 }
12752 } else {
12753 tmp = MAX_int32_T;
12754 }
12755
12756 motor_Y.JD_Error = tmp;
12757 tmp_0 = motor_B.Motor_HYDG1.KP_JD;
12758 if (tmp_0 < 2.147483648E+9) {
12759 if (tmp_0 >= -2.147483648E+9) {
12760 tmp = (int32_T)tmp_0;
12761 } else {
12762 tmp = MIN_int32_T;
12763 }
12764 } else {
12765 tmp = MAX_int32_T;
12766 }
12767
12768 motor_Y.JD_Out = tmp;
12769 motor_DWork.Saturation_limit_speed = fabs((16000.0 -
12770 motor_DWork.Saturation_limit_speed) / (real_T)mul_s32_s32_s32_sat
12771 (100000, motor_Y.JD_Error)) + (motor_DWork.Saturation_limit_speed +
12772 5.0);
12773 }
12774 break;
12775 }
12776 }
12777
12778 /* End of Inport: '<Root>/System_Order' */
12779}
12780
12781/* Function for Chart: '<Root>/Chart' */
12782static void motor_XHHY(void)
12783{
12784 boolean_T guard1 = false;
12785 int32_T tmp;
12786 uint16_T tmp_0;
12787 real_T tmp_1;
12788
12789 /* Inport: '<Root>/System_Order' */
12790 /* During 'XHHY': '<S1>:149' */
12791 if (motor_U.System_Order == 3) {
12792 /* Transition: '<S1>:553' */
12793 /* Transition: '<S1>:557' */
12794 motor_exit_internal_Normal_Mode();
12795 motor_DWork.is_M_Run = motor_IN_Close;
12796 motor_DWork.is_Close = motor_IN_Close_Wait;
12797 motor_DWork.temporalCounter_i1 = 0U;
12798
12799 /* Entry 'Close_Wait': '<S1>:241' */
12800 motor_Y.Open_Result = 4U;
12801
12802 /* 正在关机 */
12803 motor_Y.DCZD = false;
12804
12805 /* 解除制动 */
12806 motor_Y.Motor_En = false;
12807
12808 /* 电机使能 */
12809 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
12810 motor_DWork.EN_extern = 0U;
12811 } else {
12812 switch (motor_DWork.is_XHHY) {
12813 case motor_IN_Sleeping_protect_1:
12814 /* During 'Sleeping_protect_1': '<S1>:231' */
12815 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
12816 /* Transition: '<S1>:527' */
12817 motor_DWork.is_XHHY = motor_IN_Sleeping_protect_2;
12818
12819 /* Entry 'Sleeping_protect_2': '<S1>:174' */
12820 motor_Y.DCZD = true;
12821
12822 /* 开电磁制动 */
12823 }
12824 break;
12825
12826 case motor_IN_Sleeping_protect_2:
12827 /* During 'Sleeping_protect_2': '<S1>:174' */
12828 if (motor_U.System_Order == 5) {
12829 /* Transition: '<S1>:530' */
12830 /* Exit 'Sleeping_protect_2': '<S1>:174' */
12831 /* 关电磁制动 */
12832 motor_Y.DCZD = false;
12833 motor_DWork.is_XHHY = motor_IN_Sleeping_protect_3;
12834 motor_DWork.temporalCounter_i1 = 0U;
12835
12836 /* Entry 'Sleeping_protect_3': '<S1>:230' */
12837 motor_Y.Motor_En = false;
12838
12839 /* 开电机 */
12840 }
12841 break;
12842
12843 case motor_IN_Sleeping_protect_3:
12844 /* During 'Sleeping_protect_3': '<S1>:230' */
12845 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
12846 /* Inport: '<Root>/Gyroscope_Choose' */
12847 /* Transition: '<S1>:532' */
12848 if (motor_U.Gyroscope_Choose == 0) {
12849 /* Transition: '<S1>:1204' */
12850 motor_DWork.is_XHHY = motor_IN_XHHY_Run;
12851
12852 /* Entry 'XHHY_Run': '<S1>:364' */
12853 motor_DWork.KG_En = 0U;
12854 motor_DWork.KG_JD = 1U;
12855 motor_DWork.EN_extern = 0U;
12856 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
12857 motor_Y.Open_Result = 1U;
12858 motor_DWork.Runing_stable = 1U;
12859 motor_DWork.P_KP = 330.0;
12860 motor_DWork.V_KP = 1.0;
12861 motor_DWork.V_KI = 10.0;
12862 } else {
12863 if (motor_U.Gyroscope_Choose == 1) {
12864 /* Transition: '<S1>:1205' */
12865 motor_DWork.is_XHHY = motor_IN_XHHY_Run1;
12866
12867 /* Entry 'XHHY_Run1': '<S1>:1198' */
12868 motor_DWork.KG_En = 0U;
12869 motor_DWork.KG_JD = 1U;
12870 motor_DWork.EN_extern = 1U;
12871 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
12872 motor_Y.Open_Result = 1U;
12873 motor_DWork.Runing_stable = 1U;
12874 motor_DWork.P_KP = 150.0;
12875 motor_DWork.V_KP = 0.5;
12876 motor_DWork.V_KI = 8.0;
12877 }
12878 }
12879 }
12880 break;
12881
12882 case motor_IN_XHHY_Error1:
12883 /* Inport: '<Root>/GXDY_State' */
12884 /* During 'XHHY_Error1': '<S1>:1140' */
12885 if (!motor_U.GXDY_State) {
12886 /* Transition: '<S1>:1141' */
12887 motor_Y.Motor_En = false;
12888 motor_DWork.is_XHHY = motor_IN_XHHY_Wait;
12889 motor_DWork.temporalCounter_i1 = 0U;
12890
12891 /* Entry 'XHHY_Wait': '<S1>:175' */
12892 motor_DWork.In_State = 2U;
12893 motor_DWork.KG_En = 1U;
12894 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
12895
12896 /* during:[PWMOUT,JD_Out,JD_Error] = Motor_XHHY(JD_In,Encode_Pos,Encode_Sp,System_Order,SGWY_In); */
12897 }
12898 break;
12899
12900 case motor_IN_XHHY_Run:
12901 /* Inport: '<Root>/Gyroscope_Choose' */
12902 /* During 'XHHY_Run': '<S1>:364' */
12903 if (motor_U.System_Order == 4) {
12904 /* Transition: '<S1>:531' */
12905 /* Exit 'XHHY_Run': '<S1>:364' */
12906 motor_DWork.Runing_stable = 0U;
12907 motor_DWork.is_XHHY = motor_IN_Sleeping_protect_1;
12908 motor_DWork.temporalCounter_i1 = 0U;
12909
12910 /* Entry 'Sleeping_protect_1': '<S1>:231' */
12911 motor_Y.Motor_En = true;
12912
12913 /* 关电机 */
12914 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
12915 } else if (motor_U.Gyroscope_Choose != 0) {
12916 /* Transition: '<S1>:1207' */
12917 /* Transition: '<S1>:1209' */
12918 /* Transition: '<S1>:557' */
12919 motor_exit_internal_Normal_Mode();
12920 motor_DWork.is_M_Run = motor_IN_Close;
12921 motor_DWork.is_Close = motor_IN_Close_Wait;
12922 motor_DWork.temporalCounter_i1 = 0U;
12923
12924 /* Entry 'Close_Wait': '<S1>:241' */
12925 motor_Y.Open_Result = 4U;
12926
12927 /* 正在关机 */
12928 motor_Y.DCZD = false;
12929
12930 /* 解除制动 */
12931 motor_Y.Motor_En = false;
12932
12933 /* 电机使能 */
12934 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
12935 motor_DWork.EN_extern = 0U;
12936 } else {
12937 /* Inport: '<Root>/JD_In' */
12938 /* 内部陀螺 */
12939 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
12940 motor_B.JD_In = motor_U.JD_In;
12941 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
12942
12943 /* Inport: '<Root>/Encode_Sp' */
12944 motor_B.Encode_Sp = motor_U.Encode_Sp;
12945 motor_B.Slect_port = motor_U.System_Order;
12946
12947 /* Inport: '<Root>/SGWY_In' */
12948 motor_B.SGWY_In = motor_U.SGWY_In;
12949
12950 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
12951 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
12952 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
12953 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
12954 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
12955 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
12956 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
12957 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
12958 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
12959 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
12960 &motor_DWork.chu_jd);
12961
12962 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
12963 tmp_1 = motor_B.Motor_XHHY.ZXF_PWM;
12964 if (tmp_1 < 65536.0) {
12965 if (tmp_1 >= 0.0) {
12966 tmp_0 = (uint16_T)tmp_1;
12967 } else {
12968 tmp_0 = 0U;
12969 }
12970 } else {
12971 tmp_0 = MAX_uint16_T;
12972 }
12973
12974 motor_Y.PWMOUT = tmp_0;
12975 tmp_1 = motor_B.Motor_XHHY.KP_JD1;
12976 if (tmp_1 < 2.147483648E+9) {
12977 if (tmp_1 >= -2.147483648E+9) {
12978 tmp = (int32_T)tmp_1;
12979 } else {
12980 tmp = MIN_int32_T;
12981 }
12982 } else {
12983 tmp = MAX_int32_T;
12984 }
12985
12986 motor_Y.JD_Out = tmp;
12987 tmp_1 = motor_B.Motor_XHHY.KP_e;
12988 if (tmp_1 < 2.147483648E+9) {
12989 if (tmp_1 >= -2.147483648E+9) {
12990 tmp = (int32_T)tmp_1;
12991 } else {
12992 tmp = MIN_int32_T;
12993 }
12994 } else {
12995 tmp = MAX_int32_T;
12996 }
12997
12998 motor_Y.JD_Error = tmp;
12999 }
13000 break;
13001
13002 case motor_IN_XHHY_Run1:
13003 /* Inport: '<Root>/Gyroscope_Choose' */
13004 /* During 'XHHY_Run1': '<S1>:1198' */
13005 if (motor_U.System_Order == 4) {
13006 /* Transition: '<S1>:1202' */
13007 /* Exit 'XHHY_Run1': '<S1>:1198' */
13008 motor_DWork.Runing_stable = 0U;
13009 motor_DWork.is_XHHY = motor_IN_Sleeping_protect_1;
13010 motor_DWork.temporalCounter_i1 = 0U;
13011
13012 /* Entry 'Sleeping_protect_1': '<S1>:231' */
13013 motor_Y.Motor_En = true;
13014
13015 /* 关电机 */
13016 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
13017 } else if (motor_U.Gyroscope_Choose != 1) {
13018 /* Transition: '<S1>:1208' */
13019 /* Transition: '<S1>:1209' */
13020 /* Transition: '<S1>:557' */
13021 motor_exit_internal_Normal_Mode();
13022 motor_DWork.is_M_Run = motor_IN_Close;
13023 motor_DWork.is_Close = motor_IN_Close_Wait;
13024 motor_DWork.temporalCounter_i1 = 0U;
13025
13026 /* Entry 'Close_Wait': '<S1>:241' */
13027 motor_Y.Open_Result = 4U;
13028
13029 /* 正在关机 */
13030 motor_Y.DCZD = false;
13031
13032 /* 解除制动 */
13033 motor_Y.Motor_En = false;
13034
13035 /* 电机使能 */
13036 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
13037 motor_DWork.EN_extern = 0U;
13038 } else {
13039 /* Inport: '<Root>/JD_In' */
13040 /* 外部陀螺 */
13041 /* Simulink Function 'Waveform_Build': '<S1>:1223' */
13042 motor_B.JD_In_c = motor_U.JD_In;
13043
13044 /* Outputs for Function Call SubSystem: '<S1>/Waveform_Build' */
13045 motor_Waveform_Build(motor_M, motor_B.JD_In_c, &motor_B.Waveform_Build,
13046 &motor_DWork.Waveform_Build,
13047 (rtP_Waveform_Build_motor *)&motor_P.Waveform_Build,
13048 &motor_DWork.T, &motor_DWork.T_Count,
13049 &motor_DWork.Temp1, &motor_DWork.Temp2);
13050
13051 /* End of Outputs for SubSystem: '<S1>/Waveform_Build' */
13052
13053 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
13054 motor_B.JD_In = motor_B.Waveform_Build.Gain2;
13055 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
13056
13057 /* Inport: '<Root>/Encode_Sp' */
13058 motor_B.Encode_Sp = motor_U.Encode_Sp;
13059 motor_B.Slect_port = motor_U.System_Order;
13060
13061 /* Inport: '<Root>/SGWY_In' */
13062 motor_B.SGWY_In = motor_U.SGWY_In;
13063
13064 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
13065 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
13066 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
13067 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
13068 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
13069 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
13070 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
13071 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
13072 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
13073 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
13074 &motor_DWork.chu_jd);
13075
13076 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
13077 tmp_1 = motor_B.Motor_XHHY.ZXF_PWM;
13078 if (tmp_1 < 65536.0) {
13079 if (tmp_1 >= 0.0) {
13080 tmp_0 = (uint16_T)tmp_1;
13081 } else {
13082 tmp_0 = 0U;
13083 }
13084 } else {
13085 tmp_0 = MAX_uint16_T;
13086 }
13087
13088 motor_Y.PWMOUT = tmp_0;
13089 tmp_1 = motor_B.Motor_XHHY.KP_JD1;
13090 if (tmp_1 < 2.147483648E+9) {
13091 if (tmp_1 >= -2.147483648E+9) {
13092 tmp = (int32_T)tmp_1;
13093 } else {
13094 tmp = MIN_int32_T;
13095 }
13096 } else {
13097 tmp = MAX_int32_T;
13098 }
13099
13100 motor_Y.JD_Out = tmp;
13101 tmp_1 = motor_B.Motor_XHHY.KP_e;
13102 if (tmp_1 < 2.147483648E+9) {
13103 if (tmp_1 >= -2.147483648E+9) {
13104 tmp = (int32_T)tmp_1;
13105 } else {
13106 tmp = MIN_int32_T;
13107 }
13108 } else {
13109 tmp = MAX_int32_T;
13110 }
13111
13112 motor_Y.JD_Error = tmp;
13113 }
13114 break;
13115
13116 default:
13117 /* During 'XHHY_Wait': '<S1>:175' */
13118 if (motor_DWork.temporalCounter_i1 >= 500U) {
13119 /* Inport: '<Root>/Gyroscope_Choose' */
13120 /* Transition: '<S1>:954' */
13121 if (motor_U.Gyroscope_Choose == 0) {
13122 /* Inport: '<Root>/GXDY_State' */
13123 /* Transition: '<S1>:533' */
13124 if (!motor_U.GXDY_State) {
13125 /* Transition: '<S1>:1200' */
13126 /* 惯性单元拉低 */
13127 motor_DWork.is_XHHY = motor_IN_XHHY_Run;
13128
13129 /* Entry 'XHHY_Run': '<S1>:364' */
13130 motor_DWork.KG_En = 0U;
13131 motor_DWork.KG_JD = 1U;
13132 motor_DWork.EN_extern = 0U;
13133 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
13134 motor_Y.Open_Result = 1U;
13135 motor_DWork.Runing_stable = 1U;
13136 motor_DWork.P_KP = 330.0;
13137 motor_DWork.V_KP = 1.0;
13138 motor_DWork.V_KI = 10.0;
13139 } else {
13140 /* Transition: '<S1>:981' */
13141 motor_DWork.is_XHHY = motor_IN_XHHY_Error1;
13142
13143 /* Entry 'XHHY_Error1': '<S1>:1140' */
13144 motor_Y.Motor_En = true;
13145
13146 /* 关电机 */
13147 motor_Y.Open_Result = 2U;
13148 }
13149 } else {
13150 guard1 = true;
13151 }
13152 }
13153 break;
13154 }
13155
13156 /* Inport: '<Root>/Gyroscope_Choose' */
13157 if (guard1 && (motor_U.Gyroscope_Choose == 1)) {
13158 /* Transition: '<S1>:1201' */
13159 motor_DWork.is_XHHY = motor_IN_XHHY_Run1;
13160
13161 /* Entry 'XHHY_Run1': '<S1>:1198' */
13162 motor_DWork.KG_En = 0U;
13163 motor_DWork.KG_JD = 1U;
13164 motor_DWork.EN_extern = 1U;
13165 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
13166 motor_Y.Open_Result = 1U;
13167 motor_DWork.Runing_stable = 1U;
13168 motor_DWork.P_KP = 150.0;
13169 motor_DWork.V_KP = 0.5;
13170 motor_DWork.V_KI = 8.0;
13171 }
13172 }
13173
13174 /* End of Inport: '<Root>/System_Order' */
13175}
13176
13177/* Function for Chart: '<Root>/Chart' */
13178static void motor_Algorithm_a(void)
13179{
13180 boolean_T guard1 = false;
13181 int32_T tmp;
13182 uint16_T tmp_0;
13183 real_T tmp_1;
13184
13185 /* During 'Algorithm': '<S1>:165' */
13186 switch (motor_DWork.is_Algorithm_l) {
13187 case motor_IN_Defualt:
13188 /* Inport: '<Root>/Motor_Num' */
13189 /* During 'Defualt': '<S1>:147' */
13190 if (motor_U.Motor_Num == 3) {
13191 /* Transition: '<S1>:552' */
13192 motor_DWork.is_Algorithm_l = motor_IN_XHHY;
13193
13194 /* Entry Internal 'XHHY': '<S1>:149' */
13195 /* Transition: '<S1>:526' */
13196 motor_DWork.is_XHHY = motor_IN_XHHY_Wait;
13197 motor_DWork.temporalCounter_i1 = 0U;
13198
13199 /* Entry 'XHHY_Wait': '<S1>:175' */
13200 motor_DWork.In_State = 2U;
13201 motor_DWork.KG_En = 1U;
13202 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
13203
13204 /* during:[PWMOUT,JD_Out,JD_Error] = Motor_XHHY(JD_In,Encode_Pos,Encode_Sp,System_Order,SGWY_In); */
13205 } else if (motor_U.Motor_Num == 2) {
13206 /* Transition: '<S1>:554' */
13207 motor_DWork.is_Algorithm_l = motor_IN_HY;
13208
13209 /* Entry Internal 'HY': '<S1>:167' */
13210 /* Transition: '<S1>:562' */
13211 motor_DWork.is_HY = motor_IN_XHHY_Wait;
13212 motor_DWork.temporalCounter_i1 = 0U;
13213
13214 /* Entry 'XHHY_Wait': '<S1>:178' */
13215 /* Open_Result=uint8(1);
13216 开机状态位成功 */
13217 motor_DWork.In_State = 2U;
13218 motor_Y.PWMOUT = 2500U;
13219
13220 /* during:
13221 [PWMOUT,JD_Error,JD_Out] = Motor_HYDG(JD_In,Encode_Pos,System_Order,Encode_Sp); */
13222 } else {
13223 if (motor_U.Motor_Num == 1) {
13224 /* Transition: '<S1>:555' */
13225 motor_DWork.is_Algorithm_l = motor_IN_XHZY;
13226
13227 /* Entry Internal 'XHZY': '<S1>:172' */
13228 /* Transition: '<S1>:572' */
13229 motor_DWork.is_XHZY = motor_IN_XHZY_Wait;
13230 motor_DWork.temporalCounter_i1 = 0U;
13231
13232 /* Entry 'XHZY_Wait': '<S1>:177' */
13233 motor_DWork.In_State = 2U;
13234 motor_DWork.KG_En = 1U;
13235 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
13236
13237 /* during:[PWMOUT,JD_Out,JD_Error,SGWY_Out] = Motor_XHZY(JD_In,Encode_Pos,YJ_In,Encode_Sp,System_Order); */
13238 }
13239 }
13240
13241 /* End of Inport: '<Root>/Motor_Num' */
13242 break;
13243
13244 case motor_IN_HY:
13245 motor_HY();
13246 break;
13247
13248 case motor_IN_XHHY:
13249 motor_XHHY();
13250 break;
13251
13252 default:
13253 /* Inport: '<Root>/System_Order' */
13254 /* During 'XHZY': '<S1>:172' */
13255 if (motor_U.System_Order == 3) {
13256 /* Transition: '<S1>:559' */
13257 /* Transition: '<S1>:557' */
13258 motor_exit_internal_Normal_Mode();
13259 motor_DWork.is_M_Run = motor_IN_Close;
13260 motor_DWork.is_Close = motor_IN_Close_Wait;
13261 motor_DWork.temporalCounter_i1 = 0U;
13262
13263 /* Entry 'Close_Wait': '<S1>:241' */
13264 motor_Y.Open_Result = 4U;
13265
13266 /* 正在关机 */
13267 motor_Y.DCZD = false;
13268
13269 /* 解除制动 */
13270 motor_Y.Motor_En = false;
13271
13272 /* 电机使能 */
13273 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
13274 motor_DWork.EN_extern = 0U;
13275 } else {
13276 switch (motor_DWork.is_XHZY) {
13277 case motor_IN_Sleeping_protect_1:
13278 /* During 'Sleeping_protect_1': '<S1>:235' */
13279 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts))
13280 {
13281 /* Transition: '<S1>:575' */
13282 motor_DWork.is_XHZY = motor_IN_Sleeping_protect_2;
13283
13284 /* Entry 'Sleeping_protect_2': '<S1>:236' */
13285 motor_Y.DCZD = true;
13286
13287 /* 开电磁制动 */
13288 }
13289 break;
13290
13291 case motor_IN_Sleeping_protect_2:
13292 /* During 'Sleeping_protect_2': '<S1>:236' */
13293 if (motor_U.System_Order == 5) {
13294 /* Transition: '<S1>:576' */
13295 /* Exit 'Sleeping_protect_2': '<S1>:236' */
13296 /* 关电磁制动 */
13297 motor_Y.DCZD = false;
13298 motor_DWork.is_XHZY = motor_IN_Sleeping_protect_3;
13299 motor_DWork.temporalCounter_i1 = 0U;
13300
13301 /* Entry 'Sleeping_protect_3': '<S1>:234' */
13302 motor_Y.Motor_En = false;
13303
13304 /* 开电机 */
13305 }
13306 break;
13307
13308 case motor_IN_Sleeping_protect_3:
13309 /* During 'Sleeping_protect_3': '<S1>:234' */
13310 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts))
13311 {
13312 /* Inport: '<Root>/Gyroscope_Choose' */
13313 /* Transition: '<S1>:579' */
13314 if (motor_U.Gyroscope_Choose == 0) {
13315 /* Transition: '<S1>:1196' */
13316 motor_DWork.is_XHZY = motor_IN_XHZY_Run;
13317 motor_enter_atomic_XHZY_Run();
13318 } else {
13319 if (motor_U.Gyroscope_Choose == 1) {
13320 /* Transition: '<S1>:1197' */
13321 motor_DWork.is_XHZY = motor_IN_XHZY_Run1;
13322 motor_enter_atomic_XHZY_Run1();
13323 }
13324 }
13325 }
13326 break;
13327
13328 case motor_IN_XHZY_Error1:
13329 /* Inport: '<Root>/GXDY_State' */
13330 /* During 'XHZY_Error1': '<S1>:1142' */
13331 if (!motor_U.GXDY_State) {
13332 /* Transition: '<S1>:1143' */
13333 motor_Y.Motor_En = false;
13334 motor_DWork.is_XHZY = motor_IN_XHZY_Wait;
13335 motor_DWork.temporalCounter_i1 = 0U;
13336
13337 /* Entry 'XHZY_Wait': '<S1>:177' */
13338 motor_DWork.In_State = 2U;
13339 motor_DWork.KG_En = 1U;
13340 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
13341
13342 /* during:[PWMOUT,JD_Out,JD_Error,SGWY_Out] = Motor_XHZY(JD_In,Encode_Pos,YJ_In,Encode_Sp,System_Order); */
13343 }
13344 break;
13345
13346 case motor_IN_XHZY_Run:
13347 /* Inport: '<Root>/Gyroscope_Choose' */
13348 /* During 'XHZY_Run': '<S1>:150' */
13349 if (motor_U.System_Order == 4) {
13350 /* Transition: '<S1>:577' */
13351 /* Exit 'XHZY_Run': '<S1>:150' */
13352 motor_DWork.Runing_stable = 0U;
13353 motor_DWork.is_XHZY = motor_IN_Sleeping_protect_1;
13354 motor_DWork.temporalCounter_i1 = 0U;
13355
13356 /* Entry 'Sleeping_protect_1': '<S1>:235' */
13357 motor_Y.Motor_En = true;
13358
13359 /* 关电机 */
13360 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
13361 } else if (motor_U.Gyroscope_Choose != 0) {
13362 /* Transition: '<S1>:1185' */
13363 /* Transition: '<S1>:1191' */
13364 /* Transition: '<S1>:557' */
13365 motor_exit_internal_Normal_Mode();
13366 motor_DWork.is_M_Run = motor_IN_Close;
13367 motor_DWork.is_Close = motor_IN_Close_Wait;
13368 motor_DWork.temporalCounter_i1 = 0U;
13369
13370 /* Entry 'Close_Wait': '<S1>:241' */
13371 motor_Y.Open_Result = 4U;
13372
13373 /* 正在关机 */
13374 motor_Y.DCZD = false;
13375
13376 /* 解除制动 */
13377 motor_Y.Motor_En = false;
13378
13379 /* 电机使能 */
13380 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
13381 motor_DWork.EN_extern = 0U;
13382 } else {
13383 /* Inport: '<Root>/JD_In' */
13384 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
13385 motor_B.JD_In_d = motor_U.JD_In;
13386 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
13387
13388 /* Inport: '<Root>/YJ_In' */
13389 motor_B.YJ_In = motor_U.YJ_In;
13390
13391 /* Inport: '<Root>/Encode_Sp' */
13392 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
13393 motor_B.Slect_port_h = motor_U.System_Order;
13394
13395 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
13396 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
13397 motor_B.YJ_In, motor_B.Encode_Sp_l,
13398 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
13399 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
13400 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
13401 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
13402 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
13403 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
13404 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
13405 &motor_DWork.KG_clc, &motor_DWork.P_KP,
13406 &motor_DWork.V_KI, &motor_DWork.V_KP,
13407 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
13408 &motor_DWork.chu_jd);
13409
13410 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
13411 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
13412 if (tmp_1 < 65536.0) {
13413 if (tmp_1 >= 0.0) {
13414 tmp_0 = (uint16_T)tmp_1;
13415 } else {
13416 tmp_0 = 0U;
13417 }
13418 } else {
13419 tmp_0 = MAX_uint16_T;
13420 }
13421
13422 motor_Y.PWMOUT = tmp_0;
13423 tmp_1 = motor_B.Motor_XHZY.KP_JD;
13424 if (tmp_1 < 2.147483648E+9) {
13425 if (tmp_1 >= -2.147483648E+9) {
13426 tmp = (int32_T)tmp_1;
13427 } else {
13428 tmp = MIN_int32_T;
13429 }
13430 } else {
13431 tmp = MAX_int32_T;
13432 }
13433
13434 motor_Y.JD_Out = tmp;
13435 tmp_1 = motor_B.Motor_XHZY.KP_e;
13436 if (tmp_1 < 2.147483648E+9) {
13437 if (tmp_1 >= -2.147483648E+9) {
13438 tmp = (int32_T)tmp_1;
13439 } else {
13440 tmp = MIN_int32_T;
13441 }
13442 } else {
13443 tmp = MAX_int32_T;
13444 }
13445
13446 motor_Y.JD_Error = tmp;
13447 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
13448 }
13449 break;
13450
13451 case motor_IN_XHZY_Run1:
13452 /* Inport: '<Root>/Gyroscope_Choose' */
13453 /* During 'XHZY_Run1': '<S1>:1182' */
13454 if (motor_U.Gyroscope_Choose != 1) {
13455 /* Transition: '<S1>:1187' */
13456 /* Transition: '<S1>:1191' */
13457 /* Transition: '<S1>:557' */
13458 motor_exit_internal_Normal_Mode();
13459 motor_DWork.is_M_Run = motor_IN_Close;
13460 motor_DWork.is_Close = motor_IN_Close_Wait;
13461 motor_DWork.temporalCounter_i1 = 0U;
13462
13463 /* Entry 'Close_Wait': '<S1>:241' */
13464 motor_Y.Open_Result = 4U;
13465
13466 /* 正在关机 */
13467 motor_Y.DCZD = false;
13468
13469 /* 解除制动 */
13470 motor_Y.Motor_En = false;
13471
13472 /* 电机使能 */
13473 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
13474 motor_DWork.EN_extern = 0U;
13475 } else if (motor_U.System_Order == 4) {
13476 /* Transition: '<S1>:1194' */
13477 /* Exit 'XHZY_Run1': '<S1>:1182' */
13478 motor_DWork.Runing_stable = 0U;
13479 motor_DWork.is_XHZY = motor_IN_Sleeping_protect_1;
13480 motor_DWork.temporalCounter_i1 = 0U;
13481
13482 /* Entry 'Sleeping_protect_1': '<S1>:235' */
13483 motor_Y.Motor_En = true;
13484
13485 /* 关电机 */
13486 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
13487 } else {
13488 /* Inport: '<Root>/JD_In' */
13489 /* Simulink Function 'Waveform_Build': '<S1>:1223' */
13490 motor_B.JD_In_c = motor_U.JD_In;
13491
13492 /* Outputs for Function Call SubSystem: '<S1>/Waveform_Build' */
13493 motor_Waveform_Build(motor_M, motor_B.JD_In_c, &motor_B.Waveform_Build,
13494 &motor_DWork.Waveform_Build,
13495 (rtP_Waveform_Build_motor *)
13496 &motor_P.Waveform_Build, &motor_DWork.T,
13497 &motor_DWork.T_Count, &motor_DWork.Temp1,
13498 &motor_DWork.Temp2);
13499
13500 /* End of Outputs for SubSystem: '<S1>/Waveform_Build' */
13501
13502 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
13503 motor_B.JD_In_d = motor_B.Waveform_Build.Gain2;
13504 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
13505
13506 /* Inport: '<Root>/YJ_In' */
13507 motor_B.YJ_In = motor_U.YJ_In;
13508
13509 /* Inport: '<Root>/Encode_Sp' */
13510 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
13511 motor_B.Slect_port_h = motor_U.System_Order;
13512
13513 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
13514 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
13515 motor_B.YJ_In, motor_B.Encode_Sp_l,
13516 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
13517 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
13518 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
13519 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
13520 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
13521 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
13522 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
13523 &motor_DWork.KG_clc, &motor_DWork.P_KP,
13524 &motor_DWork.V_KI, &motor_DWork.V_KP,
13525 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
13526 &motor_DWork.chu_jd);
13527
13528 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
13529 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
13530 if (tmp_1 < 65536.0) {
13531 if (tmp_1 >= 0.0) {
13532 tmp_0 = (uint16_T)tmp_1;
13533 } else {
13534 tmp_0 = 0U;
13535 }
13536 } else {
13537 tmp_0 = MAX_uint16_T;
13538 }
13539
13540 motor_Y.PWMOUT = tmp_0;
13541 tmp_1 = motor_B.Motor_XHZY.KP_JD;
13542 if (tmp_1 < 2.147483648E+9) {
13543 if (tmp_1 >= -2.147483648E+9) {
13544 tmp = (int32_T)tmp_1;
13545 } else {
13546 tmp = MIN_int32_T;
13547 }
13548 } else {
13549 tmp = MAX_int32_T;
13550 }
13551
13552 motor_Y.JD_Out = tmp;
13553 tmp_1 = motor_B.Motor_XHZY.KP_e;
13554 if (tmp_1 < 2.147483648E+9) {
13555 if (tmp_1 >= -2.147483648E+9) {
13556 tmp = (int32_T)tmp_1;
13557 } else {
13558 tmp = MIN_int32_T;
13559 }
13560 } else {
13561 tmp = MAX_int32_T;
13562 }
13563
13564 motor_Y.JD_Error = tmp;
13565 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
13566 }
13567 break;
13568
13569 default:
13570 /* During 'XHZY_Wait': '<S1>:177' */
13571 if (motor_DWork.temporalCounter_i1 >= 500U) {
13572 /* Inport: '<Root>/Gyroscope_Choose' */
13573 /* Transition: '<S1>:578' */
13574 if (motor_U.Gyroscope_Choose == 0) {
13575 /* Inport: '<Root>/GXDY_State' */
13576 /* Transition: '<S1>:1183' */
13577 if (motor_U.GXDY_State) {
13578 /* Transition: '<S1>:985' */
13579 motor_DWork.is_XHZY = motor_IN_XHZY_Error1;
13580
13581 /* Entry 'XHZY_Error1': '<S1>:1142' */
13582 motor_Y.Motor_En = true;
13583
13584 /* 电机失能 */
13585 motor_Y.Open_Result = 2U;
13586 } else {
13587 /* Transition: '<S1>:580' */
13588 /* 惯性单元拉低 */
13589 motor_DWork.is_XHZY = motor_IN_XHZY_Run;
13590 motor_enter_atomic_XHZY_Run();
13591 }
13592 } else {
13593 guard1 = true;
13594 }
13595 }
13596 break;
13597 }
13598 }
13599
13600 /* End of Inport: '<Root>/System_Order' */
13601 break;
13602 }
13603
13604 /* Inport: '<Root>/Gyroscope_Choose' */
13605 if (guard1 && (motor_U.Gyroscope_Choose == 1)) {
13606 /* Transition: '<S1>:1184' */
13607 motor_DWork.is_XHZY = motor_IN_XHZY_Run1;
13608 motor_enter_atomic_XHZY_Run1();
13609 }
13610}
13611
13612/* Function for Chart: '<Root>/Chart' */
13613static void motor_RUN(void)
13614{
13615 uint32_T JD_Vary;
13616 int32_T saturatedUnaryMinus;
13617 uint16_T tmp;
13618 real_T tmp_0;
13619
13620 /* During 'RUN': '<S1>:23' */
13621 /* During 'GXDY_COM_GZ': '<S1>:162' */
13622 switch (motor_DWork.is_GXDY_COM_GZ) {
13623 case motor_IN_Defult:
13624 /* Inport: '<Root>/GXDY_State' */
13625 /* During 'Defult': '<S1>:159' */
13626 if ((motor_Y.Open_Result == 1) && (!motor_U.GXDY_State)) {
13627 /* Transition: '<S1>:546' */
13628 /* 开机成功且惯性单元打开 */
13629 motor_DWork.is_GXDY_COM_GZ = motor_IN_p7;
13630
13631 /* Entry 'p7': '<S1>:152' */
13632 motor_DWork.GXDY_i = 0U;
13633
13634 /* 计数器置零 */
13635 motor_Y.Flag_GXDY_IT = 1U;
13636
13637 /* 正常 */
13638 } else {
13639 if ((motor_DWork.temporalCounter_i3 >= (uint32_T)(8.0 / motor_DWork.Ts)) &&
13640 motor_U.GXDY_State) {
13641 /* Transition: '<S1>:547' */
13642 /* Transition: '<S1>:1084' */
13643 motor_DWork.is_GXDY_COM_GZ = motor_IN_r2;
13644
13645 /* Entry 'r2': '<S1>:151' */
13646 motor_Y.Flag_GXDY_IT = 0U;
13647
13648 /* 故障 */
13649 }
13650 }
13651 break;
13652
13653 case motor_IN_Defult1:
13654 /* During 'Defult1': '<S1>:1258' */
13655 break;
13656
13657 case motor_IN_Defult2:
13658 /* During 'Defult2': '<S1>:1282' */
13659 if (motor_DWork.temporalCounter_i3 >= (uint32_T)(3.0 / motor_DWork.Ts)) {
13660 /* Inport: '<Root>/Gyroscope_Choose' */
13661 /* Transition: '<S1>:1290' */
13662 if (motor_U.Gyroscope_Choose == 0) {
13663 /* Transition: '<S1>:1256' */
13664 motor_DWork.is_GXDY_COM_GZ = motor_IN_Defult;
13665 motor_DWork.temporalCounter_i3 = 0U;
13666 } else {
13667 if (motor_U.Gyroscope_Choose == 1) {
13668 /* Transition: '<S1>:1259' */
13669 motor_DWork.is_GXDY_COM_GZ = motor_IN_Defult1;
13670
13671 /* Entry 'Defult1': '<S1>:1258' */
13672 motor_DWork.GXDY_i = 0U;
13673 }
13674 }
13675 }
13676 break;
13677
13678 case motor_IN_p1_aq:
13679 /* During 'p1': '<S1>:1091' */
13680 if (motor_DWork.temporalCounter_i3 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
13681 /* Transition: '<S1>:1090' */
13682 motor_DWork.is_GXDY_COM_GZ = motor_IN_r2;
13683
13684 /* Entry 'r2': '<S1>:151' */
13685 motor_Y.Flag_GXDY_IT = 0U;
13686
13687 /* 故障 */
13688 } else {
13689 if (!motor_U.GXDY_State) {
13690 /* Transition: '<S1>:1092' */
13691 motor_DWork.is_GXDY_COM_GZ = motor_IN_p7;
13692
13693 /* Entry 'p7': '<S1>:152' */
13694 motor_DWork.GXDY_i = 0U;
13695
13696 /* 计数器置零 */
13697 motor_Y.Flag_GXDY_IT = 1U;
13698
13699 /* 正常 */
13700 }
13701 }
13702 break;
13703
13704 case motor_IN_p5:
13705 /* Inport: '<Root>/GXDY_IT_In' */
13706 /* During 'p5': '<S1>:145' */
13707 if (motor_U.GXDY_IT_In == 1) {
13708 /* Transition: '<S1>:549' */
13709 motor_DWork.is_GXDY_COM_GZ = motor_IN_p7;
13710
13711 /* Entry 'p7': '<S1>:152' */
13712 motor_DWork.GXDY_i = 0U;
13713
13714 /* 计数器置零 */
13715 motor_Y.Flag_GXDY_IT = 1U;
13716
13717 /* 正常 */
13718 } else {
13719 tmp_0 = 3.0 / motor_DWork.Ts;
13720 if (tmp_0 < 65536.0) {
13721 if (tmp_0 >= 0.0) {
13722 tmp = (uint16_T)tmp_0;
13723 } else {
13724 tmp = 0U;
13725 }
13726 } else {
13727 tmp = MAX_uint16_T;
13728 }
13729
13730 if (motor_DWork.GXDY_i >= tmp) {
13731 /* Transition: '<S1>:550' */
13732 /* 3S */
13733 motor_DWork.is_GXDY_COM_GZ = motor_IN_p6;
13734
13735 /* Entry 'p6': '<S1>:153' */
13736 /* 1代表正常,0代表错误 */
13737 motor_Y.Flag_GXDY_IT = 0U;
13738
13739 /* 故障 */
13740 motor_Y.Motor_En = true;
13741
13742 /* 电机失能 */
13743 } else {
13744 saturatedUnaryMinus = motor_DWork.GXDY_i + 1;
13745 if (saturatedUnaryMinus > 65535) {
13746 saturatedUnaryMinus = 65535;
13747 }
13748
13749 motor_DWork.GXDY_i = (uint16_T)saturatedUnaryMinus;
13750 }
13751 }
13752 break;
13753
13754 case motor_IN_p6:
13755 /* Inport: '<Root>/GXDY_IT_In' */
13756 /* During 'p6': '<S1>:153' */
13757 if (motor_U.GXDY_IT_In == 1) {
13758 /* Transition: '<S1>:1087' */
13759 motor_Y.Motor_En = false;
13760 motor_DWork.is_GXDY_COM_GZ = motor_IN_p7;
13761
13762 /* Entry 'p7': '<S1>:152' */
13763 motor_DWork.GXDY_i = 0U;
13764
13765 /* 计数器置零 */
13766 motor_Y.Flag_GXDY_IT = 1U;
13767
13768 /* 正常 */
13769 }
13770 break;
13771
13772 case motor_IN_p7:
13773 /* Inport: '<Root>/GXDY_State' incorporates:
13774 * Inport: '<Root>/GXDY_IT_In'
13775 */
13776 /* During 'p7': '<S1>:152' */
13777 if (motor_U.GXDY_State) {
13778 /* Transition: '<S1>:1088' */
13779 motor_DWork.is_GXDY_COM_GZ = motor_IN_p1_aq;
13780 motor_DWork.temporalCounter_i3 = 0U;
13781 } else {
13782 if (motor_U.GXDY_IT_In == 0) {
13783 /* Transition: '<S1>:548' */
13784 motor_DWork.is_GXDY_COM_GZ = motor_IN_p5;
13785
13786 /* Entry 'p5': '<S1>:145' */
13787 motor_DWork.GXDY_i = 1U;
13788 }
13789 }
13790 break;
13791
13792 default:
13793 /* Inport: '<Root>/GXDY_State' */
13794 /* During 'r2': '<S1>:151' */
13795 if (!motor_U.GXDY_State) {
13796 /* Transition: '<S1>:1080' */
13797 motor_Y.Flag_GXDY_IT = 1U;
13798 motor_DWork.is_GXDY_COM_GZ = motor_IN_Defult;
13799 motor_DWork.temporalCounter_i3 = 0U;
13800 }
13801 break;
13802 }
13803
13804 /* Inport: '<Root>/JD_In' incorporates:
13805 * Inport: '<Root>/GXDY_State'
13806 */
13807 /* During 'GXDY_SJ_BJ': '<S1>:154' */
13808 /* Simulink Function 'COM_Error': '<S1>:110' */
13809 motor_B.In = motor_U.JD_In;
13810
13811 /* Outputs for Function Call SubSystem: '<S1>/COM_Error' */
13812 /* UnitDelay: '<S6>/Unit Delay' */
13813 motor_B.UnitDelay = motor_DWork.UnitDelay_DSTATE;
13814
13815 /* Sum: '<S6>/Add' */
13816 motor_B.Add = motor_B.In - motor_B.UnitDelay;
13817
13818 /* Abs: '<S6>/Abs' */
13819 motor_B.Abs = fabs(motor_B.Add);
13820
13821 /* Update for UnitDelay: '<S6>/Unit Delay' */
13822 motor_DWork.UnitDelay_DSTATE = motor_B.In;
13823
13824 /* End of Outputs for SubSystem: '<S1>/COM_Error' */
13825 tmp_0 = motor_B.Abs;
13826 if (tmp_0 < 4.294967296E+9) {
13827 if (tmp_0 >= 0.0) {
13828 JD_Vary = (uint32_T)tmp_0;
13829 } else {
13830 JD_Vary = 0U;
13831 }
13832 } else {
13833 JD_Vary = MAX_uint32_T;
13834 }
13835
13836 switch (motor_DWork.is_GXDY_SJ_BJ) {
13837 case motor_IN_Defult:
13838 /* Inport: '<Root>/GXDY_State' */
13839 /* During 'Defult': '<S1>:155' */
13840 if ((motor_Y.Open_Result == 1) && (!motor_U.GXDY_State)) {
13841 /* Transition: '<S1>:536' */
13842 /* 开机成功且惯性单元打开 */
13843 motor_DWork.is_GXDY_SJ_BJ = motor_IN_p1_aq;
13844
13845 /* Entry 'p1': '<S1>:164' */
13846 motor_DWork.can1_i = 0U;
13847 }
13848 break;
13849
13850 case motor_IN_Defult1:
13851 /* During 'Defult1': '<S1>:1278' */
13852 break;
13853
13854 case motor_IN_Defult2:
13855 /* During 'Defult2': '<S1>:1283' */
13856 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(3.0 / motor_DWork.Ts)) {
13857 /* Inport: '<Root>/Gyroscope_Choose' */
13858 /* Transition: '<S1>:1288' */
13859 if (motor_U.Gyroscope_Choose == 0) {
13860 /* Transition: '<S1>:1280' */
13861 motor_DWork.is_GXDY_SJ_BJ = motor_IN_Defult;
13862 } else {
13863 if (motor_U.Gyroscope_Choose == 1) {
13864 /* Transition: '<S1>:1277' */
13865 motor_DWork.is_GXDY_SJ_BJ = motor_IN_Defult1;
13866
13867 /* Entry 'Defult1': '<S1>:1278' */
13868 motor_Y.Flag_GXDY_Error = 1U;
13869 }
13870 }
13871 }
13872 break;
13873
13874 case motor_IN_p1_aq:
13875 /* Inport: '<Root>/JD_In' */
13876 /* During 'p1': '<S1>:164' */
13877 if (motor_U.JD_In < 0) {
13878 saturatedUnaryMinus = motor_U.JD_In;
13879 if (saturatedUnaryMinus <= MIN_int32_T) {
13880 saturatedUnaryMinus = MAX_int32_T;
13881 } else {
13882 saturatedUnaryMinus = -saturatedUnaryMinus;
13883 }
13884 } else {
13885 saturatedUnaryMinus = motor_U.JD_In;
13886 }
13887
13888 if ((JD_Vary > motor_DWork.JD_Max) || (saturatedUnaryMinus > 60000)) {
13889 /* Transition: '<S1>:540' */
13890 /* 速度大于最大值 */
13891 motor_DWork.is_GXDY_SJ_BJ = motor_IN_p2_k;
13892
13893 /* Entry 'p2': '<S1>:141' */
13894 motor_DWork.can1_i = 1U;
13895 } else {
13896 /* 计数器置零 */
13897 motor_Y.Flag_GXDY_Error = 1U;
13898
13899 /* 惯性单元正常 */
13900 }
13901 break;
13902
13903 case motor_IN_p2_k:
13904 /* Inport: '<Root>/JD_In' */
13905 /* Inport: '<Root>/GXDY_State' */
13906 /* During 'p2': '<S1>:141' */
13907 if (motor_U.JD_In < 0) {
13908 saturatedUnaryMinus = motor_U.JD_In;
13909 if (saturatedUnaryMinus <= MIN_int32_T) {
13910 saturatedUnaryMinus = MAX_int32_T;
13911 } else {
13912 saturatedUnaryMinus = -saturatedUnaryMinus;
13913 }
13914 } else {
13915 saturatedUnaryMinus = motor_U.JD_In;
13916 }
13917
13918 if ((JD_Vary <= motor_DWork.JD_Max) && (saturatedUnaryMinus <= 60000)) {
13919 /* Transition: '<S1>:541' */
13920 motor_DWork.is_GXDY_SJ_BJ = motor_IN_p1_aq;
13921
13922 /* Entry 'p1': '<S1>:164' */
13923 motor_DWork.can1_i = 0U;
13924 } else if (motor_DWork.can1_i >= 5.0 / motor_DWork.Ts) {
13925 /* Transition: '<S1>:542' */
13926 /* 5S */
13927 motor_DWork.is_GXDY_SJ_BJ = motor_IN_p3_e;
13928
13929 /* Entry 'p3': '<S1>:148' */
13930 /* 0代表正常,1代表错误 */
13931 motor_DWork.can1_i = 0U;
13932 motor_Y.Motor_En = true;
13933
13934 /* 电机失能 */
13935 motor_Y.Flag_GXDY_Error = 0U;
13936
13937 /* 惯性单元通讯故障 */
13938 } else if (motor_U.GXDY_State) {
13939 /* Transition: '<S1>:1163' */
13940 motor_DWork.is_GXDY_SJ_BJ = motor_IN_Defult;
13941 } else {
13942 saturatedUnaryMinus = motor_DWork.can1_i + 1;
13943 if (saturatedUnaryMinus > 65535) {
13944 saturatedUnaryMinus = 65535;
13945 }
13946
13947 motor_DWork.can1_i = (uint16_T)saturatedUnaryMinus;
13948 }
13949 break;
13950
13951 case motor_IN_p3_e:
13952 /* Inport: '<Root>/JD_In' */
13953 /* During 'p3': '<S1>:148' */
13954 if (motor_U.JD_In < 0) {
13955 saturatedUnaryMinus = motor_U.JD_In;
13956 if (saturatedUnaryMinus <= MIN_int32_T) {
13957 saturatedUnaryMinus = MAX_int32_T;
13958 } else {
13959 saturatedUnaryMinus = -saturatedUnaryMinus;
13960 }
13961 } else {
13962 saturatedUnaryMinus = motor_U.JD_In;
13963 }
13964
13965 if ((JD_Vary <= motor_DWork.JD_Max) && (saturatedUnaryMinus <= 60000)) {
13966 /* Transition: '<S1>:539' */
13967 motor_DWork.is_GXDY_SJ_BJ = motor_IN_p4_n;
13968
13969 /* Entry 'p4': '<S1>:144' */
13970 motor_DWork.can1_i = 1U;
13971 }
13972 break;
13973
13974 default:
13975 /* Inport: '<Root>/JD_In' */
13976 /* Inport: '<Root>/GXDY_State' */
13977 /* During 'p4': '<S1>:144' */
13978 if (motor_U.JD_In < 0) {
13979 saturatedUnaryMinus = motor_U.JD_In;
13980 if (saturatedUnaryMinus <= MIN_int32_T) {
13981 saturatedUnaryMinus = MAX_int32_T;
13982 } else {
13983 saturatedUnaryMinus = -saturatedUnaryMinus;
13984 }
13985 } else {
13986 saturatedUnaryMinus = motor_U.JD_In;
13987 }
13988
13989 if ((JD_Vary > motor_DWork.JD_Max) || (saturatedUnaryMinus > 60000)) {
13990 /* Transition: '<S1>:538' */
13991 motor_DWork.is_GXDY_SJ_BJ = motor_IN_p3_e;
13992
13993 /* Entry 'p3': '<S1>:148' */
13994 /* 0代表正常,1代表错误 */
13995 motor_DWork.can1_i = 0U;
13996 motor_Y.Motor_En = true;
13997
13998 /* 电机失能 */
13999 motor_Y.Flag_GXDY_Error = 0U;
14000
14001 /* 惯性单元通讯故障 */
14002 } else if (motor_DWork.can1_i >= 2.0 / motor_DWork.Ts) {
14003 /* Transition: '<S1>:537' */
14004 motor_Y.Motor_En = false;
14005 motor_DWork.is_GXDY_SJ_BJ = motor_IN_p1_aq;
14006
14007 /* Entry 'p1': '<S1>:164' */
14008 motor_DWork.can1_i = 0U;
14009 } else if (motor_U.GXDY_State) {
14010 /* Transition: '<S1>:1162' */
14011 motor_DWork.is_GXDY_SJ_BJ = motor_IN_Defult;
14012 } else {
14013 saturatedUnaryMinus = motor_DWork.can1_i + 1;
14014 if (saturatedUnaryMinus > 65535) {
14015 saturatedUnaryMinus = 65535;
14016 }
14017
14018 motor_DWork.can1_i = (uint16_T)saturatedUnaryMinus;
14019 }
14020 break;
14021 }
14022
14023 /* During 'Limit_Check': '<S1>:176' */
14024 switch (motor_DWork.is_Limit_Check) {
14025 case motor_IN_defult_bq:
14026 /* During 'defult': '<S1>:161' */
14027 if (motor_DWork.temporalCounter_i4 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
14028 /* Transition: '<S1>:583' */
14029 motor_DWork.is_Limit_Check = motor_IN_p1_aqc;
14030
14031 /* Entry 'p1': '<S1>:171' */
14032 motor_Y.Flag_Down_limit = 1U;
14033 motor_Y.Flag_Up_limit = 1U;
14034
14035 /* Flag_Up_GZ_limit=1;
14036 Flag_Down_GZ_limit=1; */
14037 }
14038 break;
14039
14040 case motor_IN_p1_aqc:
14041 /* Inport: '<Root>/Up_Limit' incorporates:
14042 * Inport: '<Root>/Down_Limit'
14043 */
14044 /* During 'p1': '<S1>:171' */
14045 if (motor_U.Up_Limit == 0) {
14046 /* Transition: '<S1>:584' */
14047 motor_DWork.is_Limit_Check = motor_IN_p2_kb;
14048
14049 /* Entry 'p2': '<S1>:156' */
14050 motor_Y.Flag_Up_limit = 0U;
14051 } else {
14052 if (motor_U.Down_Limit == 0) {
14053 /* Transition: '<S1>:586' */
14054 motor_DWork.is_Limit_Check = motor_IN_p3_ex;
14055
14056 /* Entry 'p3': '<S1>:160' */
14057 motor_Y.Flag_Down_limit = 0U;
14058 }
14059 }
14060 break;
14061
14062 case motor_IN_p2_kb:
14063 /* Inport: '<Root>/Up_Limit' */
14064 /* During 'p2': '<S1>:156' */
14065 if (motor_U.Up_Limit == 1) {
14066 /* Transition: '<S1>:585' */
14067 motor_DWork.is_Limit_Check = motor_IN_p1_aqc;
14068
14069 /* Entry 'p1': '<S1>:171' */
14070 motor_Y.Flag_Down_limit = 1U;
14071 motor_Y.Flag_Up_limit = 1U;
14072
14073 /* Flag_Up_GZ_limit=1;
14074 Flag_Down_GZ_limit=1; */
14075 }
14076 break;
14077
14078 default:
14079 /* Inport: '<Root>/Down_Limit' */
14080 /* During 'p3': '<S1>:160' */
14081 if (motor_U.Down_Limit == 1) {
14082 /* Transition: '<S1>:587' */
14083 motor_DWork.is_Limit_Check = motor_IN_p1_aqc;
14084
14085 /* Entry 'p1': '<S1>:171' */
14086 motor_Y.Flag_Down_limit = 1U;
14087 motor_Y.Flag_Up_limit = 1U;
14088
14089 /* Flag_Up_GZ_limit=1;
14090 Flag_Down_GZ_limit=1; */
14091 }
14092 break;
14093 }
14094
14095 motor_Algorithm_a();
14096 if (motor_DWork.is_active_RUN != 0U) {
14097 /* During 'Error_Check': '<S1>:1294' */
14098 if (motor_DWork.is_Error_Check == motor_IN_Error_Check) {
14099 /* During 'Error_Check': '<S1>:1306' */
14100 if (motor_DWork.Runing_stable == 0) {
14101 /* Transition: '<S1>:1318' */
14102 /* Exit Internal 'Error_Check': '<S1>:1306' */
14103 motor_DWork.is_Error_Check_g = motor_IN_NO_ACTIVE_CHILD;
14104 motor_DWork.is_Error_Check = motor_IN_defult_bqy;
14105
14106 /* Entry 'defult': '<S1>:1302' */
14107 motor_Y.Flag_AngleError = 1U;
14108 } else {
14109 switch (motor_DWork.is_Error_Check_g) {
14110 case motor_IN_p1:
14111 /* During 'p1': '<S1>:1313' */
14112 if (motor_Y.JD_Error > 250) {
14113 /* Transition: '<S1>:1311' */
14114 motor_DWork.is_Error_Check_g = motor_IN_p3_exj;
14115 motor_DWork.temporalCounter_i9 = 0U;
14116 }
14117 break;
14118
14119 case motor_IN_p3_exj:
14120 /* During 'p3': '<S1>:1316' */
14121 if (motor_Y.JD_Error >= 250) {
14122 /* Transition: '<S1>:1312' */
14123 motor_DWork.is_Error_Check_g = motor_IN_p1;
14124
14125 /* Entry 'p1': '<S1>:1313' */
14126 motor_Y.Flag_AngleError = 1U;
14127 } else {
14128 if (motor_DWork.temporalCounter_i9 >= (uint32_T)(2.0 /
14129 motor_DWork.Ts)) {
14130 /* Transition: '<S1>:1319' */
14131 motor_DWork.is_Error_Check_g = motor_IN_p4_nvu;
14132 motor_DWork.temporalCounter_i9 = 0U;
14133
14134 /* Entry 'p4': '<S1>:1317' */
14135 motor_Y.Flag_AngleError = 0U;
14136 }
14137 }
14138 break;
14139
14140 default:
14141 /* During 'p4': '<S1>:1317' */
14142 if (motor_DWork.temporalCounter_i9 >= (uint32_T)(5.0 / motor_DWork.Ts))
14143 {
14144 /* Transition: '<S1>:1321' */
14145 motor_DWork.is_Error_Check_g = motor_IN_p3_exj;
14146 motor_DWork.temporalCounter_i9 = 0U;
14147 }
14148 break;
14149 }
14150 }
14151 } else {
14152 /* During 'defult': '<S1>:1302' */
14153 if (motor_DWork.Runing_stable == 1) {
14154 /* Transition: '<S1>:1296' */
14155 motor_DWork.is_Error_Check = motor_IN_Error_Check;
14156
14157 /* Entry Internal 'Error_Check': '<S1>:1306' */
14158 /* Transition: '<S1>:1307' */
14159 motor_DWork.is_Error_Check_g = motor_IN_p1;
14160
14161 /* Entry 'p1': '<S1>:1313' */
14162 motor_Y.Flag_AngleError = 1U;
14163 }
14164 }
14165 }
14166}
14167
14168/* Function for Chart: '<Root>/Chart' */
14169static void motor_Normal_Mode(void)
14170{
14171 uint32_T q0;
14172 uint32_T qY;
14173 int32_T saturatedUnaryMinus;
14174 int32_T saturatedUnaryMinus_0;
14175 int32_T saturatedUnaryMinus_1;
14176
14177 /* During 'Normal_Mode': '<S1>:13' */
14178 switch (motor_DWork.is_Normal_Mode) {
14179 case motor_IN_defult1:
14180 /* During 'defult1': '<S1>:129' */
14181 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
14182 /* Transition: '<S1>:411' */
14183 motor_DWork.is_Normal_Mode = motor_IN_defult2;
14184 motor_DWork.temporalCounter_i1 = 0U;
14185
14186 /* Entry 'defult2': '<S1>:951' */
14187 motor_Y.Motor_En = false;
14188 }
14189 break;
14190
14191 case motor_IN_defult2:
14192 /* During 'defult2': '<S1>:951' */
14193 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
14194 /* Transition: '<S1>:952' */
14195 motor_DWork.is_Normal_Mode = motor_IN_p1_a;
14196
14197 /* Entry Internal 'p1': '<S1>:14' */
14198 motor_DWork.is_active_RUN = 1U;
14199
14200 /* Entry Internal 'RUN': '<S1>:23' */
14201 /* Transition: '<S1>:421' */
14202 motor_DWork.is_active_GXDY_COM_GZ = 1U;
14203
14204 /* Entry Internal 'GXDY_COM_GZ': '<S1>:162' */
14205 /* Transition: '<S1>:545' */
14206 motor_DWork.is_GXDY_COM_GZ = motor_IN_Defult2;
14207 motor_DWork.temporalCounter_i3 = 0U;
14208 motor_DWork.is_active_GXDY_SJ_BJ = 1U;
14209
14210 /* Entry Internal 'GXDY_SJ_BJ': '<S1>:154' */
14211 /* Transition: '<S1>:535' */
14212 motor_DWork.is_GXDY_SJ_BJ = motor_IN_Defult2;
14213 motor_DWork.temporalCounter_i2 = 0U;
14214 motor_DWork.is_active_Limit_Check = 1U;
14215
14216 /* Entry Internal 'Limit_Check': '<S1>:176' */
14217 /* Transition: '<S1>:582' */
14218 motor_DWork.is_Limit_Check = motor_IN_defult_bq;
14219 motor_DWork.temporalCounter_i4 = 0U;
14220 motor_DWork.is_active_Algorithm = 1U;
14221
14222 /* Entry Internal 'Algorithm': '<S1>:165' */
14223 /* Transition: '<S1>:551' */
14224 motor_DWork.is_Algorithm_l = motor_IN_Defualt;
14225
14226 /* Entry 'Defualt': '<S1>:147' */
14227 motor_DWork.chu_jd = 0.0;
14228 motor_DWork.KG = 0U;
14229 motor_DWork.is_active_Error_Check = 1U;
14230
14231 /* Entry Internal 'Error_Check': '<S1>:1294' */
14232 /* Transition: '<S1>:1295' */
14233 motor_DWork.is_Error_Check = motor_IN_defult_bqy;
14234
14235 /* Entry 'defult': '<S1>:1302' */
14236 motor_Y.Flag_AngleError = 1U;
14237 motor_DWork.is_active_Enc_GZ = 1U;
14238
14239 /* Entry 'Enc_GZ': '<S1>:15' */
14240 /* Entry Internal 'Enc_GZ': '<S1>:15' */
14241 motor_DWork.is_active_Enc1 = 1U;
14242
14243 /* Entry Internal 'Enc1': '<S1>:20' */
14244 /* Transition: '<S1>:418' */
14245 motor_DWork.is_Enc1 = motor_IN_p1;
14246
14247 /* Entry 'p1': '<S1>:21' */
14248 motor_Y.Flag_Enc_Error = 1U;
14249 motor_DWork.is_active_Enc2 = 1U;
14250
14251 /* Entry Internal 'Enc2': '<S1>:16' */
14252 /* Transition: '<S1>:413' */
14253 motor_DWork.is_Enc2 = motor_IN_p2_kbn;
14254 motor_DWork.temporalCounter_i6 = 0U;
14255
14256 /* Entry 'p2': '<S1>:17' */
14257 motor_DWork.Enc_i = 0U;
14258 }
14259 break;
14260
14261 default:
14262 /* During 'p1': '<S1>:14' */
14263 motor_RUN();
14264 if (motor_DWork.is_Normal_Mode == motor_IN_p1_a) {
14265 /* During 'Enc_GZ': '<S1>:15' */
14266 /* During 'Enc1': '<S1>:20' */
14267 if (motor_DWork.is_Enc1 == motor_IN_p1) {
14268 /* Inport: '<Root>/Encode_Sp' */
14269 /* During 'p1': '<S1>:21' */
14270 if (motor_U.Encode_Sp < 0) {
14271 saturatedUnaryMinus_0 = motor_U.Encode_Sp;
14272 if (saturatedUnaryMinus_0 <= MIN_int32_T) {
14273 saturatedUnaryMinus_1 = MAX_int32_T;
14274 } else {
14275 saturatedUnaryMinus_1 = -saturatedUnaryMinus_0;
14276 }
14277
14278 saturatedUnaryMinus_0 = motor_U.Encode_Sp;
14279 if (saturatedUnaryMinus_0 <= MIN_int32_T) {
14280 saturatedUnaryMinus_0 = MAX_int32_T;
14281 } else {
14282 saturatedUnaryMinus_0 = -saturatedUnaryMinus_0;
14283 }
14284 } else {
14285 saturatedUnaryMinus_1 = motor_U.Encode_Sp;
14286 saturatedUnaryMinus_0 = motor_U.Encode_Sp;
14287 }
14288
14289 saturatedUnaryMinus = motor_Y.JD_Out;
14290 if (saturatedUnaryMinus < 0) {
14291 if (saturatedUnaryMinus <= MIN_int32_T) {
14292 saturatedUnaryMinus = MAX_int32_T;
14293 } else {
14294 saturatedUnaryMinus = -saturatedUnaryMinus;
14295 }
14296 }
14297
14298 if (((uint32_T)saturatedUnaryMinus_1 > motor_DWork.Encode_Sp_Max) ||
14299 (((motor_Y.PWMOUT > 3500) || (motor_Y.PWMOUT < 1500)) &&
14300 (!motor_Y.Motor_En) && ((uint32_T)saturatedUnaryMinus_0 <=
14301 motor_DWork.Encode_Sp_Min) && (saturatedUnaryMinus < 18500))) {
14302 /* Transition: '<S1>:419' */
14303 motor_DWork.is_Enc1 = motor_IN_p4_nv;
14304
14305 /* Entry 'p4': '<S1>:22' */
14306 q0 = motor_DWork.Enc_i;
14307 qY = q0 + 1U;
14308 if (qY < q0) {
14309 qY = MAX_uint32_T;
14310 }
14311
14312 motor_DWork.Enc_i = qY;
14313 }
14314 } else {
14315 /* Inport: '<Root>/Encode_Sp' */
14316 /* During 'p4': '<S1>:22' */
14317 if (motor_U.Encode_Sp < 0) {
14318 saturatedUnaryMinus_0 = motor_U.Encode_Sp;
14319 if (saturatedUnaryMinus_0 <= MIN_int32_T) {
14320 saturatedUnaryMinus_1 = MAX_int32_T;
14321 } else {
14322 saturatedUnaryMinus_1 = -saturatedUnaryMinus_0;
14323 }
14324
14325 saturatedUnaryMinus_0 = motor_U.Encode_Sp;
14326 if (saturatedUnaryMinus_0 <= MIN_int32_T) {
14327 saturatedUnaryMinus_0 = MAX_int32_T;
14328 } else {
14329 saturatedUnaryMinus_0 = -saturatedUnaryMinus_0;
14330 }
14331 } else {
14332 saturatedUnaryMinus_1 = motor_U.Encode_Sp;
14333 saturatedUnaryMinus_0 = motor_U.Encode_Sp;
14334 }
14335
14336 saturatedUnaryMinus = motor_Y.JD_Out;
14337 if (saturatedUnaryMinus < 0) {
14338 if (saturatedUnaryMinus <= MIN_int32_T) {
14339 saturatedUnaryMinus = MAX_int32_T;
14340 } else {
14341 saturatedUnaryMinus = -saturatedUnaryMinus;
14342 }
14343 }
14344
14345 if (((uint32_T)saturatedUnaryMinus_1 <= motor_DWork.Encode_Sp_Max) &&
14346 (((motor_Y.PWMOUT < 3500) && (motor_Y.PWMOUT > 1500)) ||
14347 motor_Y.Motor_En || ((uint32_T)saturatedUnaryMinus_0 >
14348 motor_DWork.Encode_Sp_Min) || (saturatedUnaryMinus > 18500))) {
14349 /* Transition: '<S1>:420' */
14350 motor_DWork.is_Enc1 = motor_IN_p1;
14351
14352 /* Entry 'p1': '<S1>:21' */
14353 motor_Y.Flag_Enc_Error = 1U;
14354 } else {
14355 q0 = motor_DWork.Enc_i;
14356 qY = q0 + 1U;
14357 if (qY < q0) {
14358 qY = MAX_uint32_T;
14359 }
14360
14361 motor_DWork.Enc_i = qY;
14362 }
14363 }
14364
14365 /* During 'Enc2': '<S1>:16' */
14366 switch (motor_DWork.is_Enc2) {
14367 case motor_IN_p2_kbn:
14368 /* During 'p2': '<S1>:17' */
14369 if ((uint32_T)(5.0 / motor_DWork.Ts) == motor_DWork.temporalCounter_i6)
14370 {
14371 /* Transition: '<S1>:415' */
14372 /* 5秒钟 */
14373 if (motor_DWork.Enc_i >= 100U) {
14374 /* Transition: '<S1>:417' */
14375 motor_DWork.is_Enc2 = motor_IN_p3_exj;
14376 motor_DWork.temporalCounter_i5 = 0U;
14377 } else {
14378 if (motor_DWork.Enc_i < 100U) {
14379 /* Transition: '<S1>:416' */
14380 motor_DWork.is_Enc2 = motor_IN_p5_m;
14381
14382 /* Entry 'p5': '<S1>:18' */
14383 motor_Y.Flag_Enc_Error = 1U;
14384 }
14385 }
14386 }
14387 break;
14388
14389 case motor_IN_p3_exj:
14390 /* During 'p3': '<S1>:19' */
14391 if (motor_DWork.temporalCounter_i5 >= (uint32_T)(20.0 / motor_DWork.Ts))
14392 {
14393 /* Transition: '<S1>:1093' */
14394 motor_DWork.is_Enc2 = motor_IN_p2_kbn;
14395 motor_DWork.temporalCounter_i6 = 0U;
14396
14397 /* Entry 'p2': '<S1>:17' */
14398 motor_DWork.Enc_i = 0U;
14399 } else {
14400 motor_Y.Flag_Enc_Error = 0U;
14401
14402 /* Motor_En=1; */
14403 }
14404 break;
14405
14406 default:
14407 /* During 'p5': '<S1>:18' */
14408 /* Transition: '<S1>:414' */
14409 motor_DWork.is_Enc2 = motor_IN_p2_kbn;
14410 motor_DWork.temporalCounter_i6 = 0U;
14411
14412 /* Entry 'p2': '<S1>:17' */
14413 motor_DWork.Enc_i = 0U;
14414 break;
14415 }
14416 }
14417 break;
14418 }
14419}
14420
14421/* Function for Chart: '<Root>/Chart' */
14422static void moto_exit_internal_Showing_Mode(void)
14423{
14424 /* Exit Internal 'Showing_Mode': '<S1>:51' */
14425 if (motor_DWork.is_Showing_Mode == motor_IN_RUN) {
14426 /* Exit Internal 'RUN': '<S1>:186' */
14427 /* Exit Internal 'Limit_Check': '<S1>:200' */
14428 motor_DWork.is_Limit_Check_j = motor_IN_NO_ACTIVE_CHILD;
14429 motor_DWork.is_active_Limit_Check_o = 0U;
14430
14431 /* Exit Internal 'Algorithm': '<S1>:205' */
14432 switch (motor_DWork.is_Algorithm_e) {
14433 case motor_IN_HY:
14434 /* Exit Internal 'HY': '<S1>:214' */
14435 if (motor_DWork.is_HY_e == motor_IN_Sleeping_protect_2_m) {
14436 /* Exit 'Sleeping_protect_2': '<S1>:179' */
14437 /* 关电磁制动 */
14438 motor_Y.DCZD = false;
14439 motor_DWork.is_HY_e = motor_IN_NO_ACTIVE_CHILD;
14440 } else {
14441 motor_DWork.is_HY_e = motor_IN_NO_ACTIVE_CHILD;
14442 }
14443
14444 motor_DWork.is_Algorithm_e = motor_IN_NO_ACTIVE_CHILD;
14445 break;
14446
14447 case motor_IN_XHHY_m:
14448 /* Exit Internal 'XHHY': '<S1>:207' */
14449 if (motor_DWork.is_XHHY_n == motor_IN_Sleeping_protect_2_m) {
14450 /* Exit 'Sleeping_protect_2': '<S1>:209' */
14451 /* 关电磁制动 */
14452 motor_Y.DCZD = false;
14453 motor_DWork.is_XHHY_n = motor_IN_NO_ACTIVE_CHILD;
14454 } else {
14455 motor_DWork.is_XHHY_n = motor_IN_NO_ACTIVE_CHILD;
14456 }
14457
14458 motor_DWork.is_Algorithm_e = motor_IN_NO_ACTIVE_CHILD;
14459 break;
14460
14461 case motor_IN_XHZY_p:
14462 /* Exit Internal 'XHZY': '<S1>:220' */
14463 if (motor_DWork.is_XHZY_h == motor_IN_Sleeping_protect_2_m) {
14464 /* Exit 'Sleeping_protect_2': '<S1>:228' */
14465 /* 关电磁制动 */
14466 motor_Y.DCZD = false;
14467 motor_DWork.is_XHZY_h = motor_IN_NO_ACTIVE_CHILD;
14468 } else {
14469 motor_DWork.is_XHZY_h = motor_IN_NO_ACTIVE_CHILD;
14470 }
14471
14472 motor_DWork.is_Algorithm_e = motor_IN_NO_ACTIVE_CHILD;
14473 break;
14474
14475 default:
14476 motor_DWork.is_Algorithm_e = motor_IN_NO_ACTIVE_CHILD;
14477
14478 /* Exit Internal 'Limit_Down_Test': '<S1>:315' */
14479 /* Exit Internal 'HY': '<S1>:324' */
14480 motor_DWork.is_HY_aj = motor_IN_NO_ACTIVE_CHILD;
14481 motor_DWork.is_Limit_Down_Test_i = motor_IN_NO_ACTIVE_CHILD;
14482
14483 /* Exit Internal 'XHHY': '<S1>:332' */
14484 motor_DWork.is_XHHY_fs = motor_IN_NO_ACTIVE_CHILD;
14485
14486 /* Exit Internal 'XHZY': '<S1>:316' */
14487 motor_DWork.is_XHZY_m = motor_IN_NO_ACTIVE_CHILD;
14488
14489 /* Exit Internal 'Limit_Up_Test': '<S1>:340' */
14490 /* Exit Internal 'HY': '<S1>:349' */
14491 motor_DWork.is_HY_l = motor_IN_NO_ACTIVE_CHILD;
14492 motor_DWork.is_Limit_Up_Test_j = motor_IN_NO_ACTIVE_CHILD;
14493
14494 /* Exit Internal 'XHHY': '<S1>:357' */
14495 motor_DWork.is_XHHY_nb = motor_IN_NO_ACTIVE_CHILD;
14496
14497 /* Exit Internal 'XHZY': '<S1>:341' */
14498 motor_DWork.is_XHZY_d = motor_IN_NO_ACTIVE_CHILD;
14499 break;
14500 }
14501
14502 motor_DWork.is_active_Algorithm_b = 0U;
14503 motor_DWork.is_Showing_Mode = motor_IN_NO_ACTIVE_CHILD;
14504 } else {
14505 motor_DWork.is_Showing_Mode = motor_IN_NO_ACTIVE_CHILD;
14506 }
14507}
14508
14509/* Function for Chart: '<Root>/Chart' */
14510static void enter_internal_Limit_Down_Test(void)
14511{
14512 /* Inport: '<Root>/Motor_Num' */
14513 /* Entry Internal 'Limit_Down_Test': '<S1>:315' */
14514 /* Transition: '<S1>:739' */
14515 if (motor_U.Motor_Num == 1) {
14516 /* Transition: '<S1>:741' */
14517 motor_DWork.is_Limit_Down_Test_i = motor_IN_XHZY_pv;
14518
14519 /* Entry 'XHZY': '<S1>:316' */
14520 /* 下滑纵摇电子下限位测试 */
14521 /* Entry Internal 'XHZY': '<S1>:316' */
14522 /* Transition: '<S1>:749' */
14523 motor_DWork.is_XHZY_m = motor_IN_Int;
14524 motor_DWork.temporalCounter_i2 = 0U;
14525
14526 /* Entry 'Int': '<S1>:317' */
14527 /* Simulink Function 'Angle_Calculation_XH': '<S1>:250' */
14528 motor_B.Encode_Pos_d = motor_Y.Encode_Pos;
14529
14530 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_XH' */
14531 /* DataTypeConversion: '<S4>/Data Type Conversion2' */
14532 motor_B.DataTypeConversion2 = motor_B.Encode_Pos_d;
14533
14534 /* Gain: '<S4>/GXZ6' */
14535 motor_B.GXZ6 = motor_P.GXZ6_Gain * motor_B.DataTypeConversion2;
14536
14537 /* Gain: '<S4>/GXZ1' */
14538 motor_B.GXZ1 = motor_P.GXZ1_Gain * motor_B.GXZ6;
14539
14540 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_XH' */
14541 motor_DWork.chu_jd = motor_B.GXZ1;
14542 motor_Y.DCZD = false;
14543
14544 /* 解除制动 */
14545 motor_Y.Motor_En = false;
14546
14547 /* 电机使能 */
14548 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
14549 motor_DWork.KG = 1U;
14550 motor_DWork.KG_JD = 0U;
14551 motor_DWork.KG_YJ = 0U;
14552 motor_DWork.KG_En = 1U;
14553 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
14554 } else if (motor_U.Motor_Num == 3) {
14555 /* Transition: '<S1>:740' */
14556 motor_DWork.is_Limit_Down_Test_i = motor_IN_XHHY_m1;
14557
14558 /* Entry 'XHHY': '<S1>:332' */
14559 /* 下滑横摇电子下限位测试 */
14560 /* Entry Internal 'XHHY': '<S1>:332' */
14561 /* Transition: '<S1>:765' */
14562 motor_DWork.is_XHHY_fs = motor_IN_Int;
14563 motor_DWork.temporalCounter_i2 = 0U;
14564
14565 /* Entry 'Int': '<S1>:333' */
14566 /* Simulink Function 'Angle_Calculation_XH': '<S1>:250' */
14567 motor_B.Encode_Pos_d = motor_Y.Encode_Pos;
14568
14569 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_XH' */
14570 /* DataTypeConversion: '<S4>/Data Type Conversion2' */
14571 motor_B.DataTypeConversion2 = motor_B.Encode_Pos_d;
14572
14573 /* Gain: '<S4>/GXZ6' */
14574 motor_B.GXZ6 = motor_P.GXZ6_Gain * motor_B.DataTypeConversion2;
14575
14576 /* Gain: '<S4>/GXZ1' */
14577 motor_B.GXZ1 = motor_P.GXZ1_Gain * motor_B.GXZ6;
14578
14579 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_XH' */
14580 motor_DWork.chu_jd = motor_B.GXZ1;
14581 motor_Y.DCZD = false;
14582
14583 /* 解除制动 */
14584 motor_Y.Motor_En = false;
14585
14586 /* 电机使能 */
14587 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
14588 motor_DWork.KG = 1U;
14589 motor_DWork.KG_JD = 0U;
14590 motor_DWork.KG_En = 1U;
14591 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
14592 } else {
14593 if (motor_U.Motor_Num == 2) {
14594 /* Transition: '<S1>:742' */
14595 motor_DWork.is_Limit_Down_Test_i = motor_IN_HY_f;
14596
14597 /* Entry 'HY': '<S1>:324' */
14598 /* 横摇灯杆电子下限位测试 */
14599 /* Entry Internal 'HY': '<S1>:324' */
14600 /* Transition: '<S1>:757' */
14601 motor_DWork.is_HY_aj = motor_IN_Int;
14602 motor_DWork.temporalCounter_i2 = 0U;
14603
14604 /* Entry 'Int': '<S1>:325' */
14605 /* Simulink Function 'Angle_Calculation_HY': '<S1>:253' */
14606 motor_B.Encode_Pos_dc = motor_Y.Encode_Pos;
14607
14608 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_HY' */
14609 motor_Angle_Calculation_HY(motor_B.Encode_Pos_dc,
14610 &motor_B.Angle_Calculation_HY, (rtP_Angle_Calculation_HY_motor *)
14611 &motor_P.Angle_Calculation_HY);
14612
14613 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_HY' */
14614 motor_DWork.chu_jd = motor_B.Angle_Calculation_HY.GHDG9;
14615 motor_Y.DCZD = false;
14616
14617 /* 解除制动 */
14618 motor_Y.Motor_En = false;
14619
14620 /* 电机使能 */
14621 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
14622 motor_DWork.KG = 1U;
14623 motor_DWork.KG_JD = 0U;
14624 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
14625 }
14626 }
14627
14628 /* End of Inport: '<Root>/Motor_Num' */
14629}
14630
14631/* Function for Chart: '<Root>/Chart' */
14632static void mo_enter_internal_Limit_Up_Test(void)
14633{
14634 /* Inport: '<Root>/Motor_Num' */
14635 /* Entry Internal 'Limit_Up_Test': '<S1>:340' */
14636 /* Transition: '<S1>:774' */
14637 if (motor_U.Motor_Num == 1) {
14638 /* Transition: '<S1>:776' */
14639 motor_DWork.is_Limit_Up_Test_j = motor_IN_XHZY_pv;
14640
14641 /* Entry 'XHZY': '<S1>:341' */
14642 /* 下滑纵摇电子上限位测试 */
14643 /* Entry Internal 'XHZY': '<S1>:341' */
14644 /* Transition: '<S1>:784' */
14645 motor_DWork.is_XHZY_d = motor_IN_Int;
14646 motor_DWork.temporalCounter_i2 = 0U;
14647
14648 /* Entry 'Int': '<S1>:342' */
14649 /* Simulink Function 'Angle_Calculation_XH': '<S1>:250' */
14650 motor_B.Encode_Pos_d = motor_Y.Encode_Pos;
14651
14652 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_XH' */
14653 /* DataTypeConversion: '<S4>/Data Type Conversion2' */
14654 motor_B.DataTypeConversion2 = motor_B.Encode_Pos_d;
14655
14656 /* Gain: '<S4>/GXZ6' */
14657 motor_B.GXZ6 = motor_P.GXZ6_Gain * motor_B.DataTypeConversion2;
14658
14659 /* Gain: '<S4>/GXZ1' */
14660 motor_B.GXZ1 = motor_P.GXZ1_Gain * motor_B.GXZ6;
14661
14662 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_XH' */
14663 motor_DWork.chu_jd = motor_B.GXZ1;
14664 motor_Y.DCZD = false;
14665
14666 /* 解除制动 */
14667 motor_Y.Motor_En = false;
14668
14669 /* 电机使能 */
14670 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
14671 motor_DWork.KG = 1U;
14672 motor_DWork.KG_JD = 0U;
14673 motor_DWork.KG_YJ = 0U;
14674 motor_DWork.KG_En = 1U;
14675 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
14676 } else if (motor_U.Motor_Num == 3) {
14677 /* Transition: '<S1>:775' */
14678 motor_DWork.is_Limit_Up_Test_j = motor_IN_XHHY_m1;
14679
14680 /* Entry 'XHHY': '<S1>:357' */
14681 /* 下滑横摇电子上限位测试 */
14682 /* Entry Internal 'XHHY': '<S1>:357' */
14683 /* Transition: '<S1>:800' */
14684 motor_DWork.is_XHHY_nb = motor_IN_Int;
14685 motor_DWork.temporalCounter_i2 = 0U;
14686
14687 /* Entry 'Int': '<S1>:358' */
14688 /* Simulink Function 'Angle_Calculation_XH': '<S1>:250' */
14689 motor_B.Encode_Pos_d = motor_Y.Encode_Pos;
14690
14691 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_XH' */
14692 /* DataTypeConversion: '<S4>/Data Type Conversion2' */
14693 motor_B.DataTypeConversion2 = motor_B.Encode_Pos_d;
14694
14695 /* Gain: '<S4>/GXZ6' */
14696 motor_B.GXZ6 = motor_P.GXZ6_Gain * motor_B.DataTypeConversion2;
14697
14698 /* Gain: '<S4>/GXZ1' */
14699 motor_B.GXZ1 = motor_P.GXZ1_Gain * motor_B.GXZ6;
14700
14701 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_XH' */
14702 motor_DWork.chu_jd = motor_B.GXZ1;
14703 motor_Y.DCZD = false;
14704
14705 /* 解除制动 */
14706 motor_Y.Motor_En = false;
14707
14708 /* 电机使能 */
14709 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
14710 motor_DWork.KG = 1U;
14711 motor_DWork.KG_JD = 0U;
14712 motor_DWork.KG_En = 1U;
14713 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
14714 } else {
14715 if (motor_U.Motor_Num == 2) {
14716 /* Transition: '<S1>:777' */
14717 motor_DWork.is_Limit_Up_Test_j = motor_IN_HY_f;
14718
14719 /* Entry 'HY': '<S1>:349' */
14720 /* 调试模式 */
14721 /* Entry Internal 'HY': '<S1>:349' */
14722 /* Transition: '<S1>:792' */
14723 motor_DWork.is_HY_l = motor_IN_Int;
14724 motor_DWork.temporalCounter_i2 = 0U;
14725
14726 /* Entry 'Int': '<S1>:350' */
14727 /* Simulink Function 'Angle_Calculation_HY': '<S1>:253' */
14728 motor_B.Encode_Pos_dc = motor_Y.Encode_Pos;
14729
14730 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_HY' */
14731 motor_Angle_Calculation_HY(motor_B.Encode_Pos_dc,
14732 &motor_B.Angle_Calculation_HY, (rtP_Angle_Calculation_HY_motor *)
14733 &motor_P.Angle_Calculation_HY);
14734
14735 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_HY' */
14736 motor_DWork.chu_jd = motor_B.Angle_Calculation_HY.GHDG9;
14737 motor_Y.DCZD = false;
14738
14739 /* 解除制动 */
14740 motor_Y.Motor_En = false;
14741
14742 /* 电机使能 */
14743 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
14744 motor_DWork.KG = 1U;
14745 motor_DWork.KG_JD = 0U;
14746 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
14747 }
14748 }
14749
14750 /* End of Inport: '<Root>/Motor_Num' */
14751}
14752
14753/* Function for Chart: '<Root>/Chart' */
14754static void motor_XHZY(void)
14755{
14756 uint16_T tmp;
14757 real_T tmp_0;
14758 int32_T saturatedUnaryMinus;
14759
14760 /* Inport: '<Root>/Test_Mode' */
14761 /* During 'XHZY': '<S1>:220' */
14762 if (motor_U.Test_Mode == 2) {
14763 /* Transition: '<S1>:629' */
14764 /* Transition: '<S1>:631' */
14765 motor_DWork.XiaoDaShen = 1.0;
14766
14767 /* Exit Internal 'XHZY': '<S1>:220' */
14768 if (motor_DWork.is_XHZY_h == motor_IN_Sleeping_protect_2_m) {
14769 /* Exit 'Sleeping_protect_2': '<S1>:228' */
14770 /* 关电磁制动 */
14771 motor_Y.DCZD = false;
14772 motor_DWork.is_XHZY_h = motor_IN_NO_ACTIVE_CHILD;
14773 } else {
14774 motor_DWork.is_XHZY_h = motor_IN_NO_ACTIVE_CHILD;
14775 }
14776
14777 motor_DWork.is_Algorithm_e = motor_IN_Limit_Up_Test_k;
14778
14779 /* Entry 'Limit_Up_Test': '<S1>:340' */
14780 /* 测试电子上限位 */
14781 mo_enter_internal_Limit_Up_Test();
14782 } else if (motor_U.Test_Mode == 3) {
14783 /* Transition: '<S1>:630' */
14784 /* Transition: '<S1>:634' */
14785 motor_DWork.XiaoDaShen = 1.0;
14786
14787 /* Exit Internal 'XHZY': '<S1>:220' */
14788 if (motor_DWork.is_XHZY_h == motor_IN_Sleeping_protect_2_m) {
14789 /* Exit 'Sleeping_protect_2': '<S1>:228' */
14790 /* 关电磁制动 */
14791 motor_Y.DCZD = false;
14792 motor_DWork.is_XHZY_h = motor_IN_NO_ACTIVE_CHILD;
14793 } else {
14794 motor_DWork.is_XHZY_h = motor_IN_NO_ACTIVE_CHILD;
14795 }
14796
14797 motor_DWork.is_Algorithm_e = motor_IN_Limit_Down_Test_f;
14798
14799 /* Entry 'Limit_Down_Test': '<S1>:315' */
14800 /* 电子下限位检测 */
14801 enter_internal_Limit_Down_Test();
14802 } else {
14803 switch (motor_DWork.is_XHZY_h) {
14804 case motor_IN_Defualt:
14805 /* During 'Defualt': '<S1>:221' */
14806 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(15.0 / motor_DWork.Ts)) {
14807 /* Transition: '<S1>:656' */
14808 /* 等待15s */
14809 motor_DWork.is_XHZY_h = motor_IN_XHZY_Error_fb;
14810
14811 /* Entry 'XHZY_Error': '<S1>:222' */
14812 motor_Y.Motor_En = true;
14813
14814 /* 电机失能 */
14815 motor_Y.Open_Result = 2U;
14816 } else {
14817 saturatedUnaryMinus = motor_Y.Encode_Pos;
14818 if (saturatedUnaryMinus < 0) {
14819 if (saturatedUnaryMinus <= MIN_int32_T) {
14820 saturatedUnaryMinus = MAX_int32_T;
14821 } else {
14822 saturatedUnaryMinus = -saturatedUnaryMinus;
14823 }
14824 }
14825
14826 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Pos_Zero) {
14827 /* Transition: '<S1>:655' */
14828 motor_DWork.is_XHZY_h = motor_IN_XHZY_Wait_l;
14829 motor_DWork.temporalCounter_i2 = 0U;
14830
14831 /* Entry 'XHZY_Wait': '<S1>:224' */
14832 motor_Y.Open_Result = 1U;
14833
14834 /* 开机状态位成功 */
14835 motor_DWork.In_State = 2U;
14836 motor_DWork.KG_En = 1U;
14837 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
14838 } else {
14839 /* Inport: '<Root>/JD_In' */
14840 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
14841 motor_B.JD_In_d = motor_U.JD_In;
14842 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
14843
14844 /* Inport: '<Root>/YJ_In' */
14845 motor_B.YJ_In = motor_U.YJ_In;
14846
14847 /* Inport: '<Root>/Encode_Sp' */
14848 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
14849
14850 /* Inport: '<Root>/System_Order' */
14851 motor_B.Slect_port_h = motor_U.System_Order;
14852
14853 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
14854 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
14855 motor_B.YJ_In, motor_B.Encode_Sp_l,
14856 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
14857 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
14858 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
14859 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
14860 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
14861 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
14862 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
14863 &motor_DWork.KG_clc, &motor_DWork.P_KP,
14864 &motor_DWork.V_KI, &motor_DWork.V_KP,
14865 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
14866 &motor_DWork.chu_jd);
14867
14868 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
14869 tmp_0 = motor_B.Motor_XHZY.ZXF_PWM;
14870 if (tmp_0 < 65536.0) {
14871 if (tmp_0 >= 0.0) {
14872 tmp = (uint16_T)tmp_0;
14873 } else {
14874 tmp = 0U;
14875 }
14876 } else {
14877 tmp = MAX_uint16_T;
14878 }
14879
14880 motor_Y.PWMOUT = tmp;
14881 tmp_0 = motor_B.Motor_XHZY.KP_JD;
14882 if (tmp_0 < 2.147483648E+9) {
14883 if (tmp_0 >= -2.147483648E+9) {
14884 saturatedUnaryMinus = (int32_T)tmp_0;
14885 } else {
14886 saturatedUnaryMinus = MIN_int32_T;
14887 }
14888 } else {
14889 saturatedUnaryMinus = MAX_int32_T;
14890 }
14891
14892 motor_Y.JD_Out = saturatedUnaryMinus;
14893 tmp_0 = motor_B.Motor_XHZY.KP_e;
14894 if (tmp_0 < 2.147483648E+9) {
14895 if (tmp_0 >= -2.147483648E+9) {
14896 saturatedUnaryMinus = (int32_T)tmp_0;
14897 } else {
14898 saturatedUnaryMinus = MIN_int32_T;
14899 }
14900 } else {
14901 saturatedUnaryMinus = MAX_int32_T;
14902 }
14903
14904 motor_Y.JD_Error = saturatedUnaryMinus;
14905 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
14906 }
14907 }
14908 break;
14909
14910 case motor_IN_Sleeping_protect_1_h:
14911 /* During 'Sleeping_protect_1': '<S1>:227' */
14912 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
14913 /* Transition: '<S1>:657' */
14914 motor_DWork.is_XHZY_h = motor_IN_Sleeping_protect_2_m;
14915
14916 /* Entry 'Sleeping_protect_2': '<S1>:228' */
14917 motor_Y.DCZD = true;
14918
14919 /* 开电磁制动 */
14920 }
14921 break;
14922
14923 case motor_IN_Sleeping_protect_2_m:
14924 /* Inport: '<Root>/System_Order' */
14925 /* During 'Sleeping_protect_2': '<S1>:228' */
14926 if (motor_U.System_Order == 5) {
14927 /* Transition: '<S1>:658' */
14928 /* Exit 'Sleeping_protect_2': '<S1>:228' */
14929 /* 关电磁制动 */
14930 motor_Y.DCZD = false;
14931 motor_DWork.is_XHZY_h = motor_IN_Sleeping_protect_3_f;
14932 motor_DWork.temporalCounter_i2 = 0U;
14933
14934 /* Entry 'Sleeping_protect_3': '<S1>:216' */
14935 motor_Y.Motor_En = false;
14936
14937 /* 开电机 */
14938 }
14939 break;
14940
14941 case motor_IN_Sleeping_protect_3_f:
14942 /* During 'Sleeping_protect_3': '<S1>:216' */
14943 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
14944 /* Transition: '<S1>:661' */
14945 motor_DWork.is_XHZY_h = motor_IN_XHZY_Run_i;
14946
14947 /* Entry 'XHZY_Run': '<S1>:225' */
14948 motor_DWork.KG_En = 1U;
14949 motor_DWork.KG_JD = 0U;
14950 motor_DWork.KG_YJ = 1U;
14951 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
14952
14953 /* Inport: '<Root>/Showing_Angle' */
14954 /* KG_YJ=uint8(1);%接入外部接口 */
14955 motor_DWork.Showing_Angle_Last = motor_U.Showing_Angle;
14956
14957 /* Inport: '<Root>/Showing_T' */
14958 motor_DWork.Showing_T_Last = motor_U.Showing_T;
14959 }
14960 break;
14961
14962 case motor_IN_XHZY_Error_fb:
14963 /* During 'XHZY_Error': '<S1>:222' */
14964 break;
14965
14966 case motor_IN_XHZY_Run_i:
14967 /* Inport: '<Root>/System_Order' incorporates:
14968 * Inport: '<Root>/Showing_Angle'
14969 * Inport: '<Root>/Showing_T'
14970 */
14971 /* During 'XHZY_Run': '<S1>:225' */
14972 if (motor_U.System_Order == 4) {
14973 /* Transition: '<S1>:659' */
14974 motor_DWork.is_XHZY_h = motor_IN_Sleeping_protect_1_h;
14975 motor_DWork.temporalCounter_i2 = 0U;
14976
14977 /* Entry 'Sleeping_protect_1': '<S1>:227' */
14978 motor_Y.Motor_En = true;
14979
14980 /* 关电机 */
14981 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
14982 } else if (motor_U.System_Order == 3) {
14983 /* Transition: '<S1>:662' */
14984 /* Transition: '<S1>:625' */
14985 moto_exit_internal_Showing_Mode();
14986 motor_DWork.is_M_Run = motor_IN_Close;
14987 motor_DWork.is_Close = motor_IN_Close_Wait;
14988 motor_DWork.temporalCounter_i1 = 0U;
14989
14990 /* Entry 'Close_Wait': '<S1>:241' */
14991 motor_Y.Open_Result = 4U;
14992
14993 /* 正在关机 */
14994 motor_Y.DCZD = false;
14995
14996 /* 解除制动 */
14997 motor_Y.Motor_En = false;
14998
14999 /* 电机使能 */
15000 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
15001 motor_DWork.EN_extern = 0U;
15002 } else if ((motor_DWork.Showing_Angle_Last != motor_U.Showing_Angle) ||
15003 (motor_DWork.Showing_T_Last != motor_U.Showing_T)) {
15004 /* Transition: '<S1>:950' */
15005 motor_DWork.is_XHZY_h = motor_IN_Defualt;
15006 motor_DWork.temporalCounter_i2 = 0U;
15007
15008 /* Entry 'Defualt': '<S1>:221' */
15009 /* en:KG_En=uint8(0);KG_JD=uint8(1);KG_YJ=uint8(1); */
15010 motor_DWork.KG_En = 1U;
15011 motor_DWork.KG_JD = 0U;
15012 motor_DWork.KG_YJ = 0U;
15013 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
15014 motor_DWork.chu_jd = 0.0;
15015 } else {
15016 /* Inport: '<Root>/Showing_Angle' */
15017 /* Simulink Function 'Showing': '<S1>:180' */
15018 motor_B.Showing_Angle = motor_U.Showing_Angle;
15019
15020 /* Inport: '<Root>/Showing_T' */
15021 motor_B.Showing_T = motor_U.Showing_T;
15022
15023 /* Outputs for Function Call SubSystem: '<S1>/Showing' */
15024 motor_Showing(motor_M, motor_B.Showing_Angle, motor_B.Showing_T,
15025 &motor_B.Showing, (rtP_Showing_motor *)&motor_P.Showing,
15026 &motor_DWork.Angle_S);
15027
15028 /* End of Outputs for SubSystem: '<S1>/Showing' */
15029 motor_DWork.chu_jd = motor_B.Showing.DataTypeConversion1;
15030
15031 /* Inport: '<Root>/JD_In' */
15032 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15033 motor_B.JD_In_d = motor_U.JD_In;
15034 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15035
15036 /* Inport: '<Root>/YJ_In' */
15037 motor_B.YJ_In = motor_U.YJ_In;
15038
15039 /* Inport: '<Root>/Encode_Sp' */
15040 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15041 motor_B.Slect_port_h = motor_U.System_Order;
15042
15043 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15044 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15045 motor_B.YJ_In, motor_B.Encode_Sp_l,
15046 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
15047 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
15048 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
15049 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
15050 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
15051 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
15052 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
15053 &motor_DWork.KG_clc, &motor_DWork.P_KP,
15054 &motor_DWork.V_KI, &motor_DWork.V_KP,
15055 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15056 &motor_DWork.chu_jd);
15057
15058 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15059 tmp_0 = motor_B.Motor_XHZY.ZXF_PWM;
15060 if (tmp_0 < 65536.0) {
15061 if (tmp_0 >= 0.0) {
15062 tmp = (uint16_T)tmp_0;
15063 } else {
15064 tmp = 0U;
15065 }
15066 } else {
15067 tmp = MAX_uint16_T;
15068 }
15069
15070 motor_Y.PWMOUT = tmp;
15071 tmp_0 = motor_B.Motor_XHZY.KP_JD;
15072 if (tmp_0 < 2.147483648E+9) {
15073 if (tmp_0 >= -2.147483648E+9) {
15074 saturatedUnaryMinus = (int32_T)tmp_0;
15075 } else {
15076 saturatedUnaryMinus = MIN_int32_T;
15077 }
15078 } else {
15079 saturatedUnaryMinus = MAX_int32_T;
15080 }
15081
15082 motor_Y.JD_Out = saturatedUnaryMinus;
15083 tmp_0 = motor_B.Motor_XHZY.KP_e;
15084 if (tmp_0 < 2.147483648E+9) {
15085 if (tmp_0 >= -2.147483648E+9) {
15086 saturatedUnaryMinus = (int32_T)tmp_0;
15087 } else {
15088 saturatedUnaryMinus = MIN_int32_T;
15089 }
15090 } else {
15091 saturatedUnaryMinus = MAX_int32_T;
15092 }
15093
15094 motor_Y.JD_Error = saturatedUnaryMinus;
15095 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15096 }
15097 break;
15098
15099 case motor_IN_XHZY_Run1_a:
15100 /* Inport: '<Root>/Showing_Angle' */
15101 /* During 'XHZY_Run1': '<S1>:944' */
15102 if (motor_DWork.Showing_Angle0 >= motor_U.Showing_Angle) {
15103 /* Transition: '<S1>:949' */
15104 motor_DWork.is_XHZY_h = motor_IN_XHZY_Run_i;
15105
15106 /* Entry 'XHZY_Run': '<S1>:225' */
15107 motor_DWork.KG_En = 1U;
15108 motor_DWork.KG_JD = 0U;
15109 motor_DWork.KG_YJ = 1U;
15110 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
15111
15112 /* KG_YJ=uint8(1);%接入外部接口 */
15113 motor_DWork.Showing_Angle_Last = motor_U.Showing_Angle;
15114
15115 /* Inport: '<Root>/Showing_T' */
15116 motor_DWork.Showing_T_Last = motor_U.Showing_T;
15117 } else {
15118 motor_DWork.Showing_Angle0 += 0.01;
15119
15120 /* Simulink Function 'Showing': '<S1>:180' */
15121 motor_B.Showing_Angle = motor_DWork.Showing_Angle0;
15122
15123 /* Inport: '<Root>/Showing_T' */
15124 motor_B.Showing_T = motor_U.Showing_T;
15125
15126 /* Outputs for Function Call SubSystem: '<S1>/Showing' */
15127 motor_Showing(motor_M, motor_B.Showing_Angle, motor_B.Showing_T,
15128 &motor_B.Showing, (rtP_Showing_motor *)&motor_P.Showing,
15129 &motor_DWork.Angle_S);
15130
15131 /* End of Outputs for SubSystem: '<S1>/Showing' */
15132 motor_DWork.chu_jd = motor_B.Showing.DataTypeConversion1;
15133
15134 /* Inport: '<Root>/JD_In' */
15135 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15136 motor_B.JD_In_d = motor_U.JD_In;
15137 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15138
15139 /* Inport: '<Root>/YJ_In' */
15140 motor_B.YJ_In = motor_U.YJ_In;
15141
15142 /* Inport: '<Root>/Encode_Sp' */
15143 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15144
15145 /* Inport: '<Root>/System_Order' */
15146 motor_B.Slect_port_h = motor_U.System_Order;
15147
15148 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15149 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15150 motor_B.YJ_In, motor_B.Encode_Sp_l,
15151 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
15152 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
15153 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
15154 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
15155 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
15156 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
15157 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
15158 &motor_DWork.KG_clc, &motor_DWork.P_KP,
15159 &motor_DWork.V_KI, &motor_DWork.V_KP,
15160 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15161 &motor_DWork.chu_jd);
15162
15163 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15164 tmp_0 = motor_B.Motor_XHZY.ZXF_PWM;
15165 if (tmp_0 < 65536.0) {
15166 if (tmp_0 >= 0.0) {
15167 tmp = (uint16_T)tmp_0;
15168 } else {
15169 tmp = 0U;
15170 }
15171 } else {
15172 tmp = MAX_uint16_T;
15173 }
15174
15175 motor_Y.PWMOUT = tmp;
15176 tmp_0 = motor_B.Motor_XHZY.KP_JD;
15177 if (tmp_0 < 2.147483648E+9) {
15178 if (tmp_0 >= -2.147483648E+9) {
15179 saturatedUnaryMinus = (int32_T)tmp_0;
15180 } else {
15181 saturatedUnaryMinus = MIN_int32_T;
15182 }
15183 } else {
15184 saturatedUnaryMinus = MAX_int32_T;
15185 }
15186
15187 motor_Y.JD_Out = saturatedUnaryMinus;
15188 tmp_0 = motor_B.Motor_XHZY.KP_e;
15189 if (tmp_0 < 2.147483648E+9) {
15190 if (tmp_0 >= -2.147483648E+9) {
15191 saturatedUnaryMinus = (int32_T)tmp_0;
15192 } else {
15193 saturatedUnaryMinus = MIN_int32_T;
15194 }
15195 } else {
15196 saturatedUnaryMinus = MAX_int32_T;
15197 }
15198
15199 motor_Y.JD_Error = saturatedUnaryMinus;
15200 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15201 }
15202 break;
15203
15204 default:
15205 /* During 'XHZY_Wait': '<S1>:224' */
15206 if (motor_DWork.temporalCounter_i2 >= 100U) {
15207 /* Transition: '<S1>:660' */
15208 motor_DWork.is_XHZY_h = motor_IN_XHZY_Run1_a;
15209
15210 /* Entry 'XHZY_Run1': '<S1>:944' */
15211 motor_DWork.KG_JD = 0U;
15212 motor_DWork.Showing_Angle0 = 0.0;
15213 } else {
15214 /* Inport: '<Root>/JD_In' */
15215 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15216 motor_B.JD_In_d = motor_U.JD_In;
15217 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15218
15219 /* Inport: '<Root>/YJ_In' */
15220 motor_B.YJ_In = motor_U.YJ_In;
15221
15222 /* Inport: '<Root>/Encode_Sp' */
15223 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15224
15225 /* Inport: '<Root>/System_Order' */
15226 motor_B.Slect_port_h = motor_U.System_Order;
15227
15228 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15229 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15230 motor_B.YJ_In, motor_B.Encode_Sp_l,
15231 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
15232 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
15233 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
15234 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
15235 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
15236 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
15237 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
15238 &motor_DWork.KG_clc, &motor_DWork.P_KP,
15239 &motor_DWork.V_KI, &motor_DWork.V_KP,
15240 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15241 &motor_DWork.chu_jd);
15242
15243 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15244 tmp_0 = motor_B.Motor_XHZY.ZXF_PWM;
15245 if (tmp_0 < 65536.0) {
15246 if (tmp_0 >= 0.0) {
15247 tmp = (uint16_T)tmp_0;
15248 } else {
15249 tmp = 0U;
15250 }
15251 } else {
15252 tmp = MAX_uint16_T;
15253 }
15254
15255 motor_Y.PWMOUT = tmp;
15256 tmp_0 = motor_B.Motor_XHZY.KP_JD;
15257 if (tmp_0 < 2.147483648E+9) {
15258 if (tmp_0 >= -2.147483648E+9) {
15259 saturatedUnaryMinus = (int32_T)tmp_0;
15260 } else {
15261 saturatedUnaryMinus = MIN_int32_T;
15262 }
15263 } else {
15264 saturatedUnaryMinus = MAX_int32_T;
15265 }
15266
15267 motor_Y.JD_Out = saturatedUnaryMinus;
15268 tmp_0 = motor_B.Motor_XHZY.KP_e;
15269 if (tmp_0 < 2.147483648E+9) {
15270 if (tmp_0 >= -2.147483648E+9) {
15271 saturatedUnaryMinus = (int32_T)tmp_0;
15272 } else {
15273 saturatedUnaryMinus = MIN_int32_T;
15274 }
15275 } else {
15276 saturatedUnaryMinus = MAX_int32_T;
15277 }
15278
15279 motor_Y.JD_Error = saturatedUnaryMinus;
15280 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15281
15282 /* PWMOUT=2500; */
15283 }
15284 break;
15285 }
15286 }
15287
15288 /* End of Inport: '<Root>/Test_Mode' */
15289}
15290
15291/* Function for Chart: '<Root>/Chart' */
15292static void motor_XHZY_asn(void)
15293{
15294 int32_T tmp;
15295 uint16_T tmp_0;
15296 real_T tmp_1;
15297
15298 /* During 'XHZY': '<S1>:316' */
15299 switch (motor_DWork.is_XHZY_m) {
15300 case motor_IN_Int:
15301 /* During 'Int': '<S1>:317' */
15302 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
15303 /* Transition: '<S1>:1018' */
15304 motor_DWork.is_XHZY_m = motor_IN_Int8;
15305 }
15306 break;
15307
15308 case motor_IN_Int1:
15309 /* Inport: '<Root>/Down_Limit' */
15310 /* During 'Int1': '<S1>:318' */
15311 if (motor_U.Down_Limit == 0) {
15312 /* Transition: '<S1>:751' */
15313 /* 上限位开关低电平有效 */
15314 motor_DWork.is_XHZY_m = motor_IN_Int2_i;
15315 motor_DWork.temporalCounter_i2 = 0U;
15316
15317 /* Entry 'Int2': '<S1>:319' */
15318 motor_DWork.chu_jd -= 0.002;
15319
15320 /* Inport: '<Root>/JD_In' */
15321 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15322 motor_B.JD_In_d = motor_U.JD_In;
15323 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15324
15325 /* Inport: '<Root>/YJ_In' */
15326 motor_B.YJ_In = motor_U.YJ_In;
15327
15328 /* Inport: '<Root>/Encode_Sp' */
15329 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15330
15331 /* Inport: '<Root>/System_Order' */
15332 motor_B.Slect_port_h = motor_U.System_Order;
15333
15334 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15335 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15336 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
15337 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
15338 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
15339 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
15340 &motor_DWork.EN_extern, &motor_DWork.Forword,
15341 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
15342 &motor_DWork.KG_En, &motor_DWork.KG_JD,
15343 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
15344 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
15345 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15346 &motor_DWork.chu_jd);
15347
15348 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15349 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
15350 if (tmp_1 < 65536.0) {
15351 if (tmp_1 >= 0.0) {
15352 tmp_0 = (uint16_T)tmp_1;
15353 } else {
15354 tmp_0 = 0U;
15355 }
15356 } else {
15357 tmp_0 = MAX_uint16_T;
15358 }
15359
15360 motor_Y.PWMOUT = tmp_0;
15361 tmp_1 = motor_B.Motor_XHZY.KP_JD;
15362 if (tmp_1 < 2.147483648E+9) {
15363 if (tmp_1 >= -2.147483648E+9) {
15364 tmp = (int32_T)tmp_1;
15365 } else {
15366 tmp = MIN_int32_T;
15367 }
15368 } else {
15369 tmp = MAX_int32_T;
15370 }
15371
15372 motor_Y.JD_Out = tmp;
15373 tmp_1 = motor_B.Motor_XHZY.KP_e;
15374 if (tmp_1 < 2.147483648E+9) {
15375 if (tmp_1 >= -2.147483648E+9) {
15376 tmp = (int32_T)tmp_1;
15377 } else {
15378 tmp = MIN_int32_T;
15379 }
15380 } else {
15381 tmp = MAX_int32_T;
15382 }
15383
15384 motor_Y.JD_Error = tmp;
15385 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15386 } else if (motor_DWork.temporalCounter_i2 >= (uint32_T)(5.0 / motor_DWork.Ts))
15387 {
15388 /* Transition: '<S1>:1071' */
15389 motor_Y.Flag_Down_GZ_limit = 0U;
15390
15391 /* 下限位开关故障$/ */
15392 motor_DWork.is_XHZY_m = motor_IN_Int2_i;
15393 motor_DWork.temporalCounter_i2 = 0U;
15394
15395 /* Entry 'Int2': '<S1>:319' */
15396 motor_DWork.chu_jd -= 0.002;
15397
15398 /* Inport: '<Root>/JD_In' */
15399 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15400 motor_B.JD_In_d = motor_U.JD_In;
15401 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15402
15403 /* Inport: '<Root>/YJ_In' */
15404 motor_B.YJ_In = motor_U.YJ_In;
15405
15406 /* Inport: '<Root>/Encode_Sp' */
15407 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15408
15409 /* Inport: '<Root>/System_Order' */
15410 motor_B.Slect_port_h = motor_U.System_Order;
15411
15412 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15413 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15414 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
15415 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
15416 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
15417 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
15418 &motor_DWork.EN_extern, &motor_DWork.Forword,
15419 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
15420 &motor_DWork.KG_En, &motor_DWork.KG_JD,
15421 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
15422 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
15423 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15424 &motor_DWork.chu_jd);
15425
15426 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15427 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
15428 if (tmp_1 < 65536.0) {
15429 if (tmp_1 >= 0.0) {
15430 tmp_0 = (uint16_T)tmp_1;
15431 } else {
15432 tmp_0 = 0U;
15433 }
15434 } else {
15435 tmp_0 = MAX_uint16_T;
15436 }
15437
15438 motor_Y.PWMOUT = tmp_0;
15439 tmp_1 = motor_B.Motor_XHZY.KP_JD;
15440 if (tmp_1 < 2.147483648E+9) {
15441 if (tmp_1 >= -2.147483648E+9) {
15442 tmp = (int32_T)tmp_1;
15443 } else {
15444 tmp = MIN_int32_T;
15445 }
15446 } else {
15447 tmp = MAX_int32_T;
15448 }
15449
15450 motor_Y.JD_Out = tmp;
15451 tmp_1 = motor_B.Motor_XHZY.KP_e;
15452 if (tmp_1 < 2.147483648E+9) {
15453 if (tmp_1 >= -2.147483648E+9) {
15454 tmp = (int32_T)tmp_1;
15455 } else {
15456 tmp = MIN_int32_T;
15457 }
15458 } else {
15459 tmp = MAX_int32_T;
15460 }
15461
15462 motor_Y.JD_Error = tmp;
15463 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15464 } else {
15465 motor_DWork.chu_jd -= 0.002;
15466
15467 /* Inport: '<Root>/JD_In' */
15468 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15469 motor_B.JD_In_d = motor_U.JD_In;
15470 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15471
15472 /* Inport: '<Root>/YJ_In' */
15473 motor_B.YJ_In = motor_U.YJ_In;
15474
15475 /* Inport: '<Root>/Encode_Sp' */
15476 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15477
15478 /* Inport: '<Root>/System_Order' */
15479 motor_B.Slect_port_h = motor_U.System_Order;
15480
15481 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15482 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15483 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
15484 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
15485 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
15486 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
15487 &motor_DWork.EN_extern, &motor_DWork.Forword,
15488 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
15489 &motor_DWork.KG_En, &motor_DWork.KG_JD,
15490 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
15491 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
15492 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15493 &motor_DWork.chu_jd);
15494
15495 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15496 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
15497 if (tmp_1 < 65536.0) {
15498 if (tmp_1 >= 0.0) {
15499 tmp_0 = (uint16_T)tmp_1;
15500 } else {
15501 tmp_0 = 0U;
15502 }
15503 } else {
15504 tmp_0 = MAX_uint16_T;
15505 }
15506
15507 motor_Y.PWMOUT = tmp_0;
15508 tmp_1 = motor_B.Motor_XHZY.KP_JD;
15509 if (tmp_1 < 2.147483648E+9) {
15510 if (tmp_1 >= -2.147483648E+9) {
15511 tmp = (int32_T)tmp_1;
15512 } else {
15513 tmp = MIN_int32_T;
15514 }
15515 } else {
15516 tmp = MAX_int32_T;
15517 }
15518
15519 motor_Y.JD_Out = tmp;
15520 tmp_1 = motor_B.Motor_XHZY.KP_e;
15521 if (tmp_1 < 2.147483648E+9) {
15522 if (tmp_1 >= -2.147483648E+9) {
15523 tmp = (int32_T)tmp_1;
15524 } else {
15525 tmp = MIN_int32_T;
15526 }
15527 } else {
15528 tmp = MAX_int32_T;
15529 }
15530
15531 motor_Y.JD_Error = tmp;
15532 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15533 }
15534
15535 /* End of Inport: '<Root>/Down_Limit' */
15536 break;
15537
15538 case motor_IN_Int2_i:
15539 /* During 'Int2': '<S1>:319' */
15540 if (motor_DWork.temporalCounter_i2 >= 50U) {
15541 /* Transition: '<S1>:753' */
15542 motor_DWork.is_XHZY_m = motor_IN_Int7;
15543 motor_DWork.temporalCounter_i2 = 0U;
15544 } else {
15545 motor_DWork.chu_jd -= 0.002;
15546
15547 /* Inport: '<Root>/JD_In' */
15548 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15549 motor_B.JD_In_d = motor_U.JD_In;
15550 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15551
15552 /* Inport: '<Root>/YJ_In' */
15553 motor_B.YJ_In = motor_U.YJ_In;
15554
15555 /* Inport: '<Root>/Encode_Sp' */
15556 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15557
15558 /* Inport: '<Root>/System_Order' */
15559 motor_B.Slect_port_h = motor_U.System_Order;
15560
15561 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15562 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15563 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
15564 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
15565 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
15566 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
15567 &motor_DWork.EN_extern, &motor_DWork.Forword,
15568 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
15569 &motor_DWork.KG_En, &motor_DWork.KG_JD,
15570 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
15571 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
15572 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15573 &motor_DWork.chu_jd);
15574
15575 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15576 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
15577 if (tmp_1 < 65536.0) {
15578 if (tmp_1 >= 0.0) {
15579 tmp_0 = (uint16_T)tmp_1;
15580 } else {
15581 tmp_0 = 0U;
15582 }
15583 } else {
15584 tmp_0 = MAX_uint16_T;
15585 }
15586
15587 motor_Y.PWMOUT = tmp_0;
15588 tmp_1 = motor_B.Motor_XHZY.KP_JD;
15589 if (tmp_1 < 2.147483648E+9) {
15590 if (tmp_1 >= -2.147483648E+9) {
15591 tmp = (int32_T)tmp_1;
15592 } else {
15593 tmp = MIN_int32_T;
15594 }
15595 } else {
15596 tmp = MAX_int32_T;
15597 }
15598
15599 motor_Y.JD_Out = tmp;
15600 tmp_1 = motor_B.Motor_XHZY.KP_e;
15601 if (tmp_1 < 2.147483648E+9) {
15602 if (tmp_1 >= -2.147483648E+9) {
15603 tmp = (int32_T)tmp_1;
15604 } else {
15605 tmp = MIN_int32_T;
15606 }
15607 } else {
15608 tmp = MAX_int32_T;
15609 }
15610
15611 motor_Y.JD_Error = tmp;
15612 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15613 }
15614 break;
15615
15616 case motor_IN_Int3_l:
15617 /* During 'Int3': '<S1>:320' */
15618 if (fabs(motor_DWork.chu_jd) < 0.1) {
15619 /* Transition: '<S1>:755' */
15620 motor_DWork.is_XHZY_m = motor_IN_Int4_b;
15621 motor_DWork.temporalCounter_i2 = 0U;
15622
15623 /* Entry 'Int4': '<S1>:322' */
15624 motor_DWork.chu_jd = 0.0;
15625
15626 /* Inport: '<Root>/JD_In' */
15627 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15628 motor_B.JD_In_d = motor_U.JD_In;
15629 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15630
15631 /* Inport: '<Root>/YJ_In' */
15632 motor_B.YJ_In = motor_U.YJ_In;
15633
15634 /* Inport: '<Root>/Encode_Sp' */
15635 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15636
15637 /* Inport: '<Root>/System_Order' */
15638 motor_B.Slect_port_h = motor_U.System_Order;
15639
15640 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15641 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15642 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
15643 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
15644 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
15645 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
15646 &motor_DWork.EN_extern, &motor_DWork.Forword,
15647 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
15648 &motor_DWork.KG_En, &motor_DWork.KG_JD,
15649 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
15650 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
15651 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15652 &motor_DWork.chu_jd);
15653
15654 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15655 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
15656 if (tmp_1 < 65536.0) {
15657 if (tmp_1 >= 0.0) {
15658 tmp_0 = (uint16_T)tmp_1;
15659 } else {
15660 tmp_0 = 0U;
15661 }
15662 } else {
15663 tmp_0 = MAX_uint16_T;
15664 }
15665
15666 motor_Y.PWMOUT = tmp_0;
15667 tmp_1 = motor_B.Motor_XHZY.KP_JD;
15668 if (tmp_1 < 2.147483648E+9) {
15669 if (tmp_1 >= -2.147483648E+9) {
15670 tmp = (int32_T)tmp_1;
15671 } else {
15672 tmp = MIN_int32_T;
15673 }
15674 } else {
15675 tmp = MAX_int32_T;
15676 }
15677
15678 motor_Y.JD_Out = tmp;
15679 tmp_1 = motor_B.Motor_XHZY.KP_e;
15680 if (tmp_1 < 2.147483648E+9) {
15681 if (tmp_1 >= -2.147483648E+9) {
15682 tmp = (int32_T)tmp_1;
15683 } else {
15684 tmp = MIN_int32_T;
15685 }
15686 } else {
15687 tmp = MAX_int32_T;
15688 }
15689
15690 motor_Y.JD_Error = tmp;
15691 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15692 } else {
15693 motor_DWork.chu_jd += 0.01;
15694
15695 /* Inport: '<Root>/JD_In' */
15696 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15697 motor_B.JD_In_d = motor_U.JD_In;
15698 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15699
15700 /* Inport: '<Root>/YJ_In' */
15701 motor_B.YJ_In = motor_U.YJ_In;
15702
15703 /* Inport: '<Root>/Encode_Sp' */
15704 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15705
15706 /* Inport: '<Root>/System_Order' */
15707 motor_B.Slect_port_h = motor_U.System_Order;
15708
15709 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15710 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15711 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
15712 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
15713 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
15714 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
15715 &motor_DWork.EN_extern, &motor_DWork.Forword,
15716 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
15717 &motor_DWork.KG_En, &motor_DWork.KG_JD,
15718 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
15719 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
15720 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15721 &motor_DWork.chu_jd);
15722
15723 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15724 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
15725 if (tmp_1 < 65536.0) {
15726 if (tmp_1 >= 0.0) {
15727 tmp_0 = (uint16_T)tmp_1;
15728 } else {
15729 tmp_0 = 0U;
15730 }
15731 } else {
15732 tmp_0 = MAX_uint16_T;
15733 }
15734
15735 motor_Y.PWMOUT = tmp_0;
15736 tmp_1 = motor_B.Motor_XHZY.KP_JD;
15737 if (tmp_1 < 2.147483648E+9) {
15738 if (tmp_1 >= -2.147483648E+9) {
15739 tmp = (int32_T)tmp_1;
15740 } else {
15741 tmp = MIN_int32_T;
15742 }
15743 } else {
15744 tmp = MAX_int32_T;
15745 }
15746
15747 motor_Y.JD_Out = tmp;
15748 tmp_1 = motor_B.Motor_XHZY.KP_e;
15749 if (tmp_1 < 2.147483648E+9) {
15750 if (tmp_1 >= -2.147483648E+9) {
15751 tmp = (int32_T)tmp_1;
15752 } else {
15753 tmp = MIN_int32_T;
15754 }
15755 } else {
15756 tmp = MAX_int32_T;
15757 }
15758
15759 motor_Y.JD_Error = tmp;
15760 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15761 }
15762 break;
15763
15764 case motor_IN_Int4_b:
15765 /* During 'Int4': '<S1>:322' */
15766 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(2.0 / motor_DWork.Ts)) {
15767 /* Transition: '<S1>:756' */
15768 motor_DWork.is_XHZY_m = motor_IN_Int5_a;
15769 motor_DWork.temporalCounter_i2 = 0U;
15770
15771 /* Entry 'Int5': '<S1>:323' */
15772 motor_Y.Motor_En = true;
15773
15774 /* 电机失能 */
15775 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
15776 } else {
15777 /* Inport: '<Root>/JD_In' */
15778 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15779 motor_B.JD_In_d = motor_U.JD_In;
15780 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15781
15782 /* Inport: '<Root>/YJ_In' */
15783 motor_B.YJ_In = motor_U.YJ_In;
15784
15785 /* Inport: '<Root>/Encode_Sp' */
15786 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15787
15788 /* Inport: '<Root>/System_Order' */
15789 motor_B.Slect_port_h = motor_U.System_Order;
15790
15791 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15792 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15793 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
15794 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
15795 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
15796 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
15797 &motor_DWork.EN_extern, &motor_DWork.Forword,
15798 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
15799 &motor_DWork.KG_En, &motor_DWork.KG_JD,
15800 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
15801 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
15802 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15803 &motor_DWork.chu_jd);
15804
15805 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15806 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
15807 if (tmp_1 < 65536.0) {
15808 if (tmp_1 >= 0.0) {
15809 tmp_0 = (uint16_T)tmp_1;
15810 } else {
15811 tmp_0 = 0U;
15812 }
15813 } else {
15814 tmp_0 = MAX_uint16_T;
15815 }
15816
15817 motor_Y.PWMOUT = tmp_0;
15818 tmp_1 = motor_B.Motor_XHZY.KP_JD;
15819 if (tmp_1 < 2.147483648E+9) {
15820 if (tmp_1 >= -2.147483648E+9) {
15821 tmp = (int32_T)tmp_1;
15822 } else {
15823 tmp = MIN_int32_T;
15824 }
15825 } else {
15826 tmp = MAX_int32_T;
15827 }
15828
15829 motor_Y.JD_Out = tmp;
15830 tmp_1 = motor_B.Motor_XHZY.KP_e;
15831 if (tmp_1 < 2.147483648E+9) {
15832 if (tmp_1 >= -2.147483648E+9) {
15833 tmp = (int32_T)tmp_1;
15834 } else {
15835 tmp = MIN_int32_T;
15836 }
15837 } else {
15838 tmp = MAX_int32_T;
15839 }
15840
15841 motor_Y.JD_Error = tmp;
15842 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15843 }
15844 break;
15845
15846 case motor_IN_Int5_a:
15847 /* During 'Int5': '<S1>:323' */
15848 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
15849 /* Transition: '<S1>:754' */
15850 motor_DWork.is_XHZY_m = motor_IN_Int6_m;
15851
15852 /* Entry 'Int6': '<S1>:321' */
15853 motor_Y.DCZD = true;
15854
15855 /* 开启制动 */
15856 }
15857 break;
15858
15859 case motor_IN_Int6_m:
15860 /* During 'Int6': '<S1>:321' */
15861 /* Transition: '<S1>:752' */
15862 /* Transition: '<S1>:738' */
15863 /* Transition: '<S1>:620' */
15864 motor_DWork.XiaoDaShen = 0.0;
15865 motor_DWork.is_XHZY_m = motor_IN_NO_ACTIVE_CHILD;
15866 motor_DWork.is_Limit_Down_Test_i = motor_IN_NO_ACTIVE_CHILD;
15867 motor_DWork.is_Algorithm_e = motor_IN_Defualt;
15868
15869 /* Entry 'Defualt': '<S1>:206' */
15870 motor_DWork.chu_jd = 0.0;
15871 motor_DWork.KG = 0U;
15872 motor_Y.Motor_En = false;
15873 motor_Y.DCZD = false;
15874
15875 /* 开电磁制动,低有效 */
15876 break;
15877
15878 case motor_IN_Int7:
15879 /* During 'Int7': '<S1>:961' */
15880 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
15881 /* Transition: '<S1>:962' */
15882 motor_DWork.is_XHZY_m = motor_IN_Int3_l;
15883
15884 /* Entry 'Int3': '<S1>:320' */
15885 motor_DWork.chu_jd += 0.01;
15886
15887 /* Inport: '<Root>/JD_In' */
15888 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15889 motor_B.JD_In_d = motor_U.JD_In;
15890 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15891
15892 /* Inport: '<Root>/YJ_In' */
15893 motor_B.YJ_In = motor_U.YJ_In;
15894
15895 /* Inport: '<Root>/Encode_Sp' */
15896 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15897
15898 /* Inport: '<Root>/System_Order' */
15899 motor_B.Slect_port_h = motor_U.System_Order;
15900
15901 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15902 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15903 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
15904 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
15905 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
15906 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
15907 &motor_DWork.EN_extern, &motor_DWork.Forword,
15908 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
15909 &motor_DWork.KG_En, &motor_DWork.KG_JD,
15910 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
15911 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
15912 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15913 &motor_DWork.chu_jd);
15914
15915 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15916 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
15917 if (tmp_1 < 65536.0) {
15918 if (tmp_1 >= 0.0) {
15919 tmp_0 = (uint16_T)tmp_1;
15920 } else {
15921 tmp_0 = 0U;
15922 }
15923 } else {
15924 tmp_0 = MAX_uint16_T;
15925 }
15926
15927 motor_Y.PWMOUT = tmp_0;
15928 tmp_1 = motor_B.Motor_XHZY.KP_JD;
15929 if (tmp_1 < 2.147483648E+9) {
15930 if (tmp_1 >= -2.147483648E+9) {
15931 tmp = (int32_T)tmp_1;
15932 } else {
15933 tmp = MIN_int32_T;
15934 }
15935 } else {
15936 tmp = MAX_int32_T;
15937 }
15938
15939 motor_Y.JD_Out = tmp;
15940 tmp_1 = motor_B.Motor_XHZY.KP_e;
15941 if (tmp_1 < 2.147483648E+9) {
15942 if (tmp_1 >= -2.147483648E+9) {
15943 tmp = (int32_T)tmp_1;
15944 } else {
15945 tmp = MIN_int32_T;
15946 }
15947 } else {
15948 tmp = MAX_int32_T;
15949 }
15950
15951 motor_Y.JD_Error = tmp;
15952 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
15953 } else {
15954 /* Inport: '<Root>/JD_In' */
15955 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
15956 motor_B.JD_In_d = motor_U.JD_In;
15957 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
15958
15959 /* Inport: '<Root>/YJ_In' */
15960 motor_B.YJ_In = motor_U.YJ_In;
15961
15962 /* Inport: '<Root>/Encode_Sp' */
15963 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
15964
15965 /* Inport: '<Root>/System_Order' */
15966 motor_B.Slect_port_h = motor_U.System_Order;
15967
15968 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
15969 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
15970 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
15971 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
15972 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
15973 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
15974 &motor_DWork.EN_extern, &motor_DWork.Forword,
15975 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
15976 &motor_DWork.KG_En, &motor_DWork.KG_JD,
15977 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
15978 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
15979 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
15980 &motor_DWork.chu_jd);
15981
15982 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
15983 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
15984 if (tmp_1 < 65536.0) {
15985 if (tmp_1 >= 0.0) {
15986 tmp_0 = (uint16_T)tmp_1;
15987 } else {
15988 tmp_0 = 0U;
15989 }
15990 } else {
15991 tmp_0 = MAX_uint16_T;
15992 }
15993
15994 motor_Y.PWMOUT = tmp_0;
15995 tmp_1 = motor_B.Motor_XHZY.KP_JD;
15996 if (tmp_1 < 2.147483648E+9) {
15997 if (tmp_1 >= -2.147483648E+9) {
15998 tmp = (int32_T)tmp_1;
15999 } else {
16000 tmp = MIN_int32_T;
16001 }
16002 } else {
16003 tmp = MAX_int32_T;
16004 }
16005
16006 motor_Y.JD_Out = tmp;
16007 tmp_1 = motor_B.Motor_XHZY.KP_e;
16008 if (tmp_1 < 2.147483648E+9) {
16009 if (tmp_1 >= -2.147483648E+9) {
16010 tmp = (int32_T)tmp_1;
16011 } else {
16012 tmp = MIN_int32_T;
16013 }
16014 } else {
16015 tmp = MAX_int32_T;
16016 }
16017
16018 motor_Y.JD_Error = tmp;
16019 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
16020 }
16021 break;
16022
16023 default:
16024 /* During 'Int8': '<S1>:1020' */
16025 if (motor_DWork.chu_jd < -16.0) {
16026 /* Transition: '<S1>:1019' */
16027 motor_DWork.is_XHZY_m = motor_IN_Int1;
16028 motor_DWork.temporalCounter_i2 = 0U;
16029
16030 /* Entry 'Int1': '<S1>:318' */
16031 motor_DWork.chu_jd -= 0.002;
16032
16033 /* Inport: '<Root>/JD_In' */
16034 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
16035 motor_B.JD_In_d = motor_U.JD_In;
16036 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
16037
16038 /* Inport: '<Root>/YJ_In' */
16039 motor_B.YJ_In = motor_U.YJ_In;
16040
16041 /* Inport: '<Root>/Encode_Sp' */
16042 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
16043
16044 /* Inport: '<Root>/System_Order' */
16045 motor_B.Slect_port_h = motor_U.System_Order;
16046
16047 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
16048 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
16049 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
16050 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
16051 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
16052 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
16053 &motor_DWork.EN_extern, &motor_DWork.Forword,
16054 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
16055 &motor_DWork.KG_En, &motor_DWork.KG_JD,
16056 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
16057 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
16058 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
16059 &motor_DWork.chu_jd);
16060
16061 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
16062 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
16063 if (tmp_1 < 65536.0) {
16064 if (tmp_1 >= 0.0) {
16065 tmp_0 = (uint16_T)tmp_1;
16066 } else {
16067 tmp_0 = 0U;
16068 }
16069 } else {
16070 tmp_0 = MAX_uint16_T;
16071 }
16072
16073 motor_Y.PWMOUT = tmp_0;
16074 tmp_1 = motor_B.Motor_XHZY.KP_JD;
16075 if (tmp_1 < 2.147483648E+9) {
16076 if (tmp_1 >= -2.147483648E+9) {
16077 tmp = (int32_T)tmp_1;
16078 } else {
16079 tmp = MIN_int32_T;
16080 }
16081 } else {
16082 tmp = MAX_int32_T;
16083 }
16084
16085 motor_Y.JD_Out = tmp;
16086 tmp_1 = motor_B.Motor_XHZY.KP_e;
16087 if (tmp_1 < 2.147483648E+9) {
16088 if (tmp_1 >= -2.147483648E+9) {
16089 tmp = (int32_T)tmp_1;
16090 } else {
16091 tmp = MIN_int32_T;
16092 }
16093 } else {
16094 tmp = MAX_int32_T;
16095 }
16096
16097 motor_Y.JD_Error = tmp;
16098 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
16099 } else if (motor_U.Down_Limit == 0) {
16100 /* Transition: '<S1>:1017' */
16101 /* 下限位开关低电平有效 */
16102 motor_DWork.is_XHZY_m = motor_IN_Int2_i;
16103 motor_DWork.temporalCounter_i2 = 0U;
16104
16105 /* Entry 'Int2': '<S1>:319' */
16106 motor_DWork.chu_jd -= 0.002;
16107
16108 /* Inport: '<Root>/JD_In' */
16109 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
16110 motor_B.JD_In_d = motor_U.JD_In;
16111 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
16112
16113 /* Inport: '<Root>/YJ_In' */
16114 motor_B.YJ_In = motor_U.YJ_In;
16115
16116 /* Inport: '<Root>/Encode_Sp' */
16117 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
16118
16119 /* Inport: '<Root>/System_Order' */
16120 motor_B.Slect_port_h = motor_U.System_Order;
16121
16122 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
16123 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
16124 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
16125 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
16126 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
16127 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
16128 &motor_DWork.EN_extern, &motor_DWork.Forword,
16129 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
16130 &motor_DWork.KG_En, &motor_DWork.KG_JD,
16131 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
16132 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
16133 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
16134 &motor_DWork.chu_jd);
16135
16136 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
16137 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
16138 if (tmp_1 < 65536.0) {
16139 if (tmp_1 >= 0.0) {
16140 tmp_0 = (uint16_T)tmp_1;
16141 } else {
16142 tmp_0 = 0U;
16143 }
16144 } else {
16145 tmp_0 = MAX_uint16_T;
16146 }
16147
16148 motor_Y.PWMOUT = tmp_0;
16149 tmp_1 = motor_B.Motor_XHZY.KP_JD;
16150 if (tmp_1 < 2.147483648E+9) {
16151 if (tmp_1 >= -2.147483648E+9) {
16152 tmp = (int32_T)tmp_1;
16153 } else {
16154 tmp = MIN_int32_T;
16155 }
16156 } else {
16157 tmp = MAX_int32_T;
16158 }
16159
16160 motor_Y.JD_Out = tmp;
16161 tmp_1 = motor_B.Motor_XHZY.KP_e;
16162 if (tmp_1 < 2.147483648E+9) {
16163 if (tmp_1 >= -2.147483648E+9) {
16164 tmp = (int32_T)tmp_1;
16165 } else {
16166 tmp = MIN_int32_T;
16167 }
16168 } else {
16169 tmp = MAX_int32_T;
16170 }
16171
16172 motor_Y.JD_Error = tmp;
16173 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
16174 } else {
16175 motor_DWork.chu_jd -= 0.01;
16176
16177 /* Inport: '<Root>/JD_In' */
16178 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
16179 motor_B.JD_In_d = motor_U.JD_In;
16180 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
16181
16182 /* Inport: '<Root>/YJ_In' */
16183 motor_B.YJ_In = motor_U.YJ_In;
16184
16185 /* Inport: '<Root>/Encode_Sp' */
16186 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
16187
16188 /* Inport: '<Root>/System_Order' */
16189 motor_B.Slect_port_h = motor_U.System_Order;
16190
16191 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
16192 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
16193 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
16194 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
16195 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
16196 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
16197 &motor_DWork.EN_extern, &motor_DWork.Forword,
16198 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
16199 &motor_DWork.KG_En, &motor_DWork.KG_JD,
16200 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
16201 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
16202 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
16203 &motor_DWork.chu_jd);
16204
16205 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
16206 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
16207 if (tmp_1 < 65536.0) {
16208 if (tmp_1 >= 0.0) {
16209 tmp_0 = (uint16_T)tmp_1;
16210 } else {
16211 tmp_0 = 0U;
16212 }
16213 } else {
16214 tmp_0 = MAX_uint16_T;
16215 }
16216
16217 motor_Y.PWMOUT = tmp_0;
16218 tmp_1 = motor_B.Motor_XHZY.KP_JD;
16219 if (tmp_1 < 2.147483648E+9) {
16220 if (tmp_1 >= -2.147483648E+9) {
16221 tmp = (int32_T)tmp_1;
16222 } else {
16223 tmp = MIN_int32_T;
16224 }
16225 } else {
16226 tmp = MAX_int32_T;
16227 }
16228
16229 motor_Y.JD_Out = tmp;
16230 tmp_1 = motor_B.Motor_XHZY.KP_e;
16231 if (tmp_1 < 2.147483648E+9) {
16232 if (tmp_1 >= -2.147483648E+9) {
16233 tmp = (int32_T)tmp_1;
16234 } else {
16235 tmp = MIN_int32_T;
16236 }
16237 } else {
16238 tmp = MAX_int32_T;
16239 }
16240
16241 motor_Y.JD_Error = tmp;
16242 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
16243 }
16244 break;
16245 }
16246}
16247
16248/* Function for Chart: '<Root>/Chart' */
16249static void motor_Limit_Down_Test_b(void)
16250{
16251 int32_T tmp;
16252 real_T tmp_0;
16253 uint16_T tmp_1;
16254
16255 /* During 'Limit_Down_Test': '<S1>:315' */
16256 switch (motor_DWork.is_Limit_Down_Test_i) {
16257 case motor_IN_HY_f:
16258 /* During 'HY': '<S1>:324' */
16259 switch (motor_DWork.is_HY_aj) {
16260 case motor_IN_Int:
16261 /* During 'Int': '<S1>:325' */
16262 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
16263 /* Transition: '<S1>:1023' */
16264 motor_DWork.is_HY_aj = motor_IN_Int8;
16265 }
16266 break;
16267
16268 case motor_IN_Int1:
16269 /* Inport: '<Root>/Down_Limit' */
16270 /* During 'Int1': '<S1>:326' */
16271 if (motor_U.Down_Limit == 0) {
16272 /* Transition: '<S1>:759' */
16273 /* 上限位开关低电平有效 */
16274 motor_DWork.is_HY_aj = motor_IN_Int2_i;
16275 motor_DWork.temporalCounter_i2 = 0U;
16276
16277 /* Entry 'Int2': '<S1>:327' */
16278 motor_DWork.chu_jd -= 0.002;
16279
16280 /* Inport: '<Root>/JD_In' */
16281 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16282 motor_B.JD_In_f = motor_U.JD_In;
16283 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16284
16285 /* Inport: '<Root>/System_Order' */
16286 motor_B.Slect_port_c = motor_U.System_Order;
16287
16288 /* Inport: '<Root>/Encode_Sp' */
16289 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16290
16291 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16292 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16293 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16294 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16295 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16296 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16297 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16298 &motor_DWork.chu_jd);
16299
16300 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16301 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16302 tmp_0 = motor_B.Motor_HYDG.KP_e;
16303 if (tmp_0 < 2.147483648E+9) {
16304 if (tmp_0 >= -2.147483648E+9) {
16305 tmp = (int32_T)tmp_0;
16306 } else {
16307 tmp = MIN_int32_T;
16308 }
16309 } else {
16310 tmp = MAX_int32_T;
16311 }
16312
16313 motor_Y.JD_Error = tmp;
16314 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16315 if (tmp_0 < 2.147483648E+9) {
16316 if (tmp_0 >= -2.147483648E+9) {
16317 tmp = (int32_T)tmp_0;
16318 } else {
16319 tmp = MIN_int32_T;
16320 }
16321 } else {
16322 tmp = MAX_int32_T;
16323 }
16324
16325 motor_Y.JD_Out = tmp;
16326 } else if (motor_DWork.temporalCounter_i2 >= (uint32_T)(7.0 /
16327 motor_DWork.Ts)) {
16328 /* Transition: '<S1>:1072' */
16329 motor_Y.Flag_Down_GZ_limit = 0U;
16330
16331 /* 下限位开关故障$/ */
16332 motor_DWork.is_HY_aj = motor_IN_Int2_i;
16333 motor_DWork.temporalCounter_i2 = 0U;
16334
16335 /* Entry 'Int2': '<S1>:327' */
16336 motor_DWork.chu_jd -= 0.002;
16337
16338 /* Inport: '<Root>/JD_In' */
16339 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16340 motor_B.JD_In_f = motor_U.JD_In;
16341 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16342
16343 /* Inport: '<Root>/System_Order' */
16344 motor_B.Slect_port_c = motor_U.System_Order;
16345
16346 /* Inport: '<Root>/Encode_Sp' */
16347 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16348
16349 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16350 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16351 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16352 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16353 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16354 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16355 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16356 &motor_DWork.chu_jd);
16357
16358 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16359 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16360 tmp_0 = motor_B.Motor_HYDG.KP_e;
16361 if (tmp_0 < 2.147483648E+9) {
16362 if (tmp_0 >= -2.147483648E+9) {
16363 tmp = (int32_T)tmp_0;
16364 } else {
16365 tmp = MIN_int32_T;
16366 }
16367 } else {
16368 tmp = MAX_int32_T;
16369 }
16370
16371 motor_Y.JD_Error = tmp;
16372 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16373 if (tmp_0 < 2.147483648E+9) {
16374 if (tmp_0 >= -2.147483648E+9) {
16375 tmp = (int32_T)tmp_0;
16376 } else {
16377 tmp = MIN_int32_T;
16378 }
16379 } else {
16380 tmp = MAX_int32_T;
16381 }
16382
16383 motor_Y.JD_Out = tmp;
16384 } else {
16385 motor_DWork.chu_jd -= 0.002;
16386
16387 /* Inport: '<Root>/JD_In' */
16388 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16389 motor_B.JD_In_f = motor_U.JD_In;
16390 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16391
16392 /* Inport: '<Root>/System_Order' */
16393 motor_B.Slect_port_c = motor_U.System_Order;
16394
16395 /* Inport: '<Root>/Encode_Sp' */
16396 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16397
16398 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16399 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16400 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16401 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16402 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16403 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16404 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16405 &motor_DWork.chu_jd);
16406
16407 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16408 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16409 tmp_0 = motor_B.Motor_HYDG.KP_e;
16410 if (tmp_0 < 2.147483648E+9) {
16411 if (tmp_0 >= -2.147483648E+9) {
16412 tmp = (int32_T)tmp_0;
16413 } else {
16414 tmp = MIN_int32_T;
16415 }
16416 } else {
16417 tmp = MAX_int32_T;
16418 }
16419
16420 motor_Y.JD_Error = tmp;
16421 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16422 if (tmp_0 < 2.147483648E+9) {
16423 if (tmp_0 >= -2.147483648E+9) {
16424 tmp = (int32_T)tmp_0;
16425 } else {
16426 tmp = MIN_int32_T;
16427 }
16428 } else {
16429 tmp = MAX_int32_T;
16430 }
16431
16432 motor_Y.JD_Out = tmp;
16433 }
16434 break;
16435
16436 case motor_IN_Int2_i:
16437 /* During 'Int2': '<S1>:327' */
16438 if (motor_DWork.temporalCounter_i2 >= 50U) {
16439 /* Transition: '<S1>:761' */
16440 motor_DWork.is_HY_aj = motor_IN_Int7;
16441 motor_DWork.temporalCounter_i2 = 0U;
16442 } else {
16443 motor_DWork.chu_jd -= 0.002;
16444
16445 /* Inport: '<Root>/JD_In' */
16446 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16447 motor_B.JD_In_f = motor_U.JD_In;
16448 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16449
16450 /* Inport: '<Root>/System_Order' */
16451 motor_B.Slect_port_c = motor_U.System_Order;
16452
16453 /* Inport: '<Root>/Encode_Sp' */
16454 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16455
16456 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16457 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16458 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16459 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16460 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16461 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16462 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16463 &motor_DWork.chu_jd);
16464
16465 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16466 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16467 tmp_0 = motor_B.Motor_HYDG.KP_e;
16468 if (tmp_0 < 2.147483648E+9) {
16469 if (tmp_0 >= -2.147483648E+9) {
16470 tmp = (int32_T)tmp_0;
16471 } else {
16472 tmp = MIN_int32_T;
16473 }
16474 } else {
16475 tmp = MAX_int32_T;
16476 }
16477
16478 motor_Y.JD_Error = tmp;
16479 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16480 if (tmp_0 < 2.147483648E+9) {
16481 if (tmp_0 >= -2.147483648E+9) {
16482 tmp = (int32_T)tmp_0;
16483 } else {
16484 tmp = MIN_int32_T;
16485 }
16486 } else {
16487 tmp = MAX_int32_T;
16488 }
16489
16490 motor_Y.JD_Out = tmp;
16491 }
16492 break;
16493
16494 case motor_IN_Int3_l:
16495 /* During 'Int3': '<S1>:328' */
16496 if (fabs(motor_DWork.chu_jd) < 0.1) {
16497 /* Transition: '<S1>:762' */
16498 motor_DWork.is_HY_aj = motor_IN_Int4_b;
16499 motor_DWork.temporalCounter_i2 = 0U;
16500
16501 /* Entry 'Int4': '<S1>:330' */
16502 motor_DWork.chu_jd = 0.0;
16503
16504 /* Inport: '<Root>/JD_In' */
16505 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16506 motor_B.JD_In_f = motor_U.JD_In;
16507 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16508
16509 /* Inport: '<Root>/System_Order' */
16510 motor_B.Slect_port_c = motor_U.System_Order;
16511
16512 /* Inport: '<Root>/Encode_Sp' */
16513 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16514
16515 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16516 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16517 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16518 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16519 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16520 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16521 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16522 &motor_DWork.chu_jd);
16523
16524 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16525 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16526 tmp_0 = motor_B.Motor_HYDG.KP_e;
16527 if (tmp_0 < 2.147483648E+9) {
16528 if (tmp_0 >= -2.147483648E+9) {
16529 tmp = (int32_T)tmp_0;
16530 } else {
16531 tmp = MIN_int32_T;
16532 }
16533 } else {
16534 tmp = MAX_int32_T;
16535 }
16536
16537 motor_Y.JD_Error = tmp;
16538 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16539 if (tmp_0 < 2.147483648E+9) {
16540 if (tmp_0 >= -2.147483648E+9) {
16541 tmp = (int32_T)tmp_0;
16542 } else {
16543 tmp = MIN_int32_T;
16544 }
16545 } else {
16546 tmp = MAX_int32_T;
16547 }
16548
16549 motor_Y.JD_Out = tmp;
16550 } else {
16551 motor_DWork.chu_jd += 0.01;
16552
16553 /* Inport: '<Root>/JD_In' */
16554 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16555 motor_B.JD_In_f = motor_U.JD_In;
16556 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16557
16558 /* Inport: '<Root>/System_Order' */
16559 motor_B.Slect_port_c = motor_U.System_Order;
16560
16561 /* Inport: '<Root>/Encode_Sp' */
16562 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16563
16564 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16565 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16566 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16567 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16568 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16569 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16570 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16571 &motor_DWork.chu_jd);
16572
16573 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16574 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16575 tmp_0 = motor_B.Motor_HYDG.KP_e;
16576 if (tmp_0 < 2.147483648E+9) {
16577 if (tmp_0 >= -2.147483648E+9) {
16578 tmp = (int32_T)tmp_0;
16579 } else {
16580 tmp = MIN_int32_T;
16581 }
16582 } else {
16583 tmp = MAX_int32_T;
16584 }
16585
16586 motor_Y.JD_Error = tmp;
16587 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16588 if (tmp_0 < 2.147483648E+9) {
16589 if (tmp_0 >= -2.147483648E+9) {
16590 tmp = (int32_T)tmp_0;
16591 } else {
16592 tmp = MIN_int32_T;
16593 }
16594 } else {
16595 tmp = MAX_int32_T;
16596 }
16597
16598 motor_Y.JD_Out = tmp;
16599 }
16600 break;
16601
16602 case motor_IN_Int4_b:
16603 /* During 'Int4': '<S1>:330' */
16604 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(2.0 / motor_DWork.Ts)) {
16605 /* Transition: '<S1>:764' */
16606 motor_DWork.is_HY_aj = motor_IN_Int5_a;
16607 motor_DWork.temporalCounter_i2 = 0U;
16608
16609 /* Entry 'Int5': '<S1>:331' */
16610 motor_Y.Motor_En = true;
16611
16612 /* 电机失能 */
16613 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
16614 } else {
16615 /* Inport: '<Root>/JD_In' */
16616 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16617 motor_B.JD_In_f = motor_U.JD_In;
16618 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16619
16620 /* Inport: '<Root>/System_Order' */
16621 motor_B.Slect_port_c = motor_U.System_Order;
16622
16623 /* Inport: '<Root>/Encode_Sp' */
16624 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16625
16626 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16627 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16628 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16629 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16630 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16631 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16632 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16633 &motor_DWork.chu_jd);
16634
16635 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16636 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16637 tmp_0 = motor_B.Motor_HYDG.KP_e;
16638 if (tmp_0 < 2.147483648E+9) {
16639 if (tmp_0 >= -2.147483648E+9) {
16640 tmp = (int32_T)tmp_0;
16641 } else {
16642 tmp = MIN_int32_T;
16643 }
16644 } else {
16645 tmp = MAX_int32_T;
16646 }
16647
16648 motor_Y.JD_Error = tmp;
16649 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16650 if (tmp_0 < 2.147483648E+9) {
16651 if (tmp_0 >= -2.147483648E+9) {
16652 tmp = (int32_T)tmp_0;
16653 } else {
16654 tmp = MIN_int32_T;
16655 }
16656 } else {
16657 tmp = MAX_int32_T;
16658 }
16659
16660 motor_Y.JD_Out = tmp;
16661 }
16662 break;
16663
16664 case motor_IN_Int5_a:
16665 /* During 'Int5': '<S1>:331' */
16666 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
16667 /* Transition: '<S1>:763' */
16668 motor_DWork.is_HY_aj = motor_IN_Int6_m;
16669
16670 /* Entry 'Int6': '<S1>:329' */
16671 motor_Y.DCZD = false;
16672
16673 /* 解除制动 */
16674 }
16675 break;
16676
16677 case motor_IN_Int6_m:
16678 /* During 'Int6': '<S1>:329' */
16679 /* Transition: '<S1>:760' */
16680 /* Transition: '<S1>:738' */
16681 /* Transition: '<S1>:620' */
16682 motor_DWork.XiaoDaShen = 0.0;
16683 motor_DWork.is_HY_aj = motor_IN_NO_ACTIVE_CHILD;
16684 motor_DWork.is_Limit_Down_Test_i = motor_IN_NO_ACTIVE_CHILD;
16685 motor_DWork.is_Algorithm_e = motor_IN_Defualt;
16686
16687 /* Entry 'Defualt': '<S1>:206' */
16688 motor_DWork.chu_jd = 0.0;
16689 motor_DWork.KG = 0U;
16690 motor_Y.Motor_En = false;
16691 motor_Y.DCZD = false;
16692
16693 /* 开电磁制动,低有效 */
16694 break;
16695
16696 case motor_IN_Int7:
16697 /* During 'Int7': '<S1>:963' */
16698 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
16699 /* Transition: '<S1>:964' */
16700 motor_DWork.is_HY_aj = motor_IN_Int3_l;
16701
16702 /* Entry 'Int3': '<S1>:328' */
16703 motor_DWork.chu_jd += 0.01;
16704
16705 /* Inport: '<Root>/JD_In' */
16706 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16707 motor_B.JD_In_f = motor_U.JD_In;
16708 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16709
16710 /* Inport: '<Root>/System_Order' */
16711 motor_B.Slect_port_c = motor_U.System_Order;
16712
16713 /* Inport: '<Root>/Encode_Sp' */
16714 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16715
16716 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16717 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16718 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16719 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16720 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16721 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16722 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16723 &motor_DWork.chu_jd);
16724
16725 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16726 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16727 tmp_0 = motor_B.Motor_HYDG.KP_e;
16728 if (tmp_0 < 2.147483648E+9) {
16729 if (tmp_0 >= -2.147483648E+9) {
16730 tmp = (int32_T)tmp_0;
16731 } else {
16732 tmp = MIN_int32_T;
16733 }
16734 } else {
16735 tmp = MAX_int32_T;
16736 }
16737
16738 motor_Y.JD_Error = tmp;
16739 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16740 if (tmp_0 < 2.147483648E+9) {
16741 if (tmp_0 >= -2.147483648E+9) {
16742 tmp = (int32_T)tmp_0;
16743 } else {
16744 tmp = MIN_int32_T;
16745 }
16746 } else {
16747 tmp = MAX_int32_T;
16748 }
16749
16750 motor_Y.JD_Out = tmp;
16751 } else {
16752 /* Inport: '<Root>/JD_In' */
16753 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16754 motor_B.JD_In_f = motor_U.JD_In;
16755 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16756
16757 /* Inport: '<Root>/System_Order' */
16758 motor_B.Slect_port_c = motor_U.System_Order;
16759
16760 /* Inport: '<Root>/Encode_Sp' */
16761 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16762
16763 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16764 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16765 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16766 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16767 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16768 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16769 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16770 &motor_DWork.chu_jd);
16771
16772 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16773 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16774 tmp_0 = motor_B.Motor_HYDG.KP_e;
16775 if (tmp_0 < 2.147483648E+9) {
16776 if (tmp_0 >= -2.147483648E+9) {
16777 tmp = (int32_T)tmp_0;
16778 } else {
16779 tmp = MIN_int32_T;
16780 }
16781 } else {
16782 tmp = MAX_int32_T;
16783 }
16784
16785 motor_Y.JD_Error = tmp;
16786 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16787 if (tmp_0 < 2.147483648E+9) {
16788 if (tmp_0 >= -2.147483648E+9) {
16789 tmp = (int32_T)tmp_0;
16790 } else {
16791 tmp = MIN_int32_T;
16792 }
16793 } else {
16794 tmp = MAX_int32_T;
16795 }
16796
16797 motor_Y.JD_Out = tmp;
16798 }
16799 break;
16800
16801 default:
16802 /* During 'Int8': '<S1>:1022' */
16803 if (motor_DWork.chu_jd < -14.0) {
16804 /* Transition: '<S1>:1024' */
16805 motor_DWork.is_HY_aj = motor_IN_Int1;
16806 motor_DWork.temporalCounter_i2 = 0U;
16807
16808 /* Entry 'Int1': '<S1>:326' */
16809 motor_DWork.chu_jd -= 0.002;
16810
16811 /* Inport: '<Root>/JD_In' */
16812 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16813 motor_B.JD_In_f = motor_U.JD_In;
16814 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16815
16816 /* Inport: '<Root>/System_Order' */
16817 motor_B.Slect_port_c = motor_U.System_Order;
16818
16819 /* Inport: '<Root>/Encode_Sp' */
16820 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16821
16822 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16823 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16824 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16825 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16826 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16827 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16828 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16829 &motor_DWork.chu_jd);
16830
16831 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16832 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16833 tmp_0 = motor_B.Motor_HYDG.KP_e;
16834 if (tmp_0 < 2.147483648E+9) {
16835 if (tmp_0 >= -2.147483648E+9) {
16836 tmp = (int32_T)tmp_0;
16837 } else {
16838 tmp = MIN_int32_T;
16839 }
16840 } else {
16841 tmp = MAX_int32_T;
16842 }
16843
16844 motor_Y.JD_Error = tmp;
16845 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16846 if (tmp_0 < 2.147483648E+9) {
16847 if (tmp_0 >= -2.147483648E+9) {
16848 tmp = (int32_T)tmp_0;
16849 } else {
16850 tmp = MIN_int32_T;
16851 }
16852 } else {
16853 tmp = MAX_int32_T;
16854 }
16855
16856 motor_Y.JD_Out = tmp;
16857 } else if (motor_U.Down_Limit == 0) {
16858 /* Transition: '<S1>:1021' */
16859 /* 下限位开关低电平有效 */
16860 motor_DWork.is_HY_aj = motor_IN_Int2_i;
16861 motor_DWork.temporalCounter_i2 = 0U;
16862
16863 /* Entry 'Int2': '<S1>:327' */
16864 motor_DWork.chu_jd -= 0.002;
16865
16866 /* Inport: '<Root>/JD_In' */
16867 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16868 motor_B.JD_In_f = motor_U.JD_In;
16869 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16870
16871 /* Inport: '<Root>/System_Order' */
16872 motor_B.Slect_port_c = motor_U.System_Order;
16873
16874 /* Inport: '<Root>/Encode_Sp' */
16875 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16876
16877 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16878 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16879 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16880 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16881 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16882 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16883 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16884 &motor_DWork.chu_jd);
16885
16886 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16887 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16888 tmp_0 = motor_B.Motor_HYDG.KP_e;
16889 if (tmp_0 < 2.147483648E+9) {
16890 if (tmp_0 >= -2.147483648E+9) {
16891 tmp = (int32_T)tmp_0;
16892 } else {
16893 tmp = MIN_int32_T;
16894 }
16895 } else {
16896 tmp = MAX_int32_T;
16897 }
16898
16899 motor_Y.JD_Error = tmp;
16900 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16901 if (tmp_0 < 2.147483648E+9) {
16902 if (tmp_0 >= -2.147483648E+9) {
16903 tmp = (int32_T)tmp_0;
16904 } else {
16905 tmp = MIN_int32_T;
16906 }
16907 } else {
16908 tmp = MAX_int32_T;
16909 }
16910
16911 motor_Y.JD_Out = tmp;
16912 } else {
16913 motor_DWork.chu_jd -= 0.01;
16914
16915 /* Inport: '<Root>/JD_In' */
16916 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
16917 motor_B.JD_In_f = motor_U.JD_In;
16918 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
16919
16920 /* Inport: '<Root>/System_Order' */
16921 motor_B.Slect_port_c = motor_U.System_Order;
16922
16923 /* Inport: '<Root>/Encode_Sp' */
16924 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
16925
16926 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
16927 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
16928 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
16929 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
16930 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
16931 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
16932 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
16933 &motor_DWork.chu_jd);
16934
16935 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
16936 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
16937 tmp_0 = motor_B.Motor_HYDG.KP_e;
16938 if (tmp_0 < 2.147483648E+9) {
16939 if (tmp_0 >= -2.147483648E+9) {
16940 tmp = (int32_T)tmp_0;
16941 } else {
16942 tmp = MIN_int32_T;
16943 }
16944 } else {
16945 tmp = MAX_int32_T;
16946 }
16947
16948 motor_Y.JD_Error = tmp;
16949 tmp_0 = motor_B.Motor_HYDG.KP_JD;
16950 if (tmp_0 < 2.147483648E+9) {
16951 if (tmp_0 >= -2.147483648E+9) {
16952 tmp = (int32_T)tmp_0;
16953 } else {
16954 tmp = MIN_int32_T;
16955 }
16956 } else {
16957 tmp = MAX_int32_T;
16958 }
16959
16960 motor_Y.JD_Out = tmp;
16961 }
16962 break;
16963 }
16964 break;
16965
16966 case motor_IN_XHHY_m1:
16967 /* During 'XHHY': '<S1>:332' */
16968 switch (motor_DWork.is_XHHY_fs) {
16969 case motor_IN_Int:
16970 /* During 'Int': '<S1>:333' */
16971 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
16972 /* Transition: '<S1>:1025' */
16973 motor_DWork.is_XHHY_fs = motor_IN_Int8;
16974 }
16975 break;
16976
16977 case motor_IN_Int1:
16978 /* Inport: '<Root>/Down_Limit' */
16979 /* During 'Int1': '<S1>:334' */
16980 if (motor_U.Down_Limit == 0) {
16981 /* Transition: '<S1>:767' */
16982 /* 上限位开关低电平有效 */
16983 motor_DWork.is_XHHY_fs = motor_IN_Int2_i;
16984 motor_DWork.temporalCounter_i2 = 0U;
16985
16986 /* Entry 'Int2': '<S1>:335' */
16987 motor_DWork.chu_jd -= 0.002;
16988
16989 /* Inport: '<Root>/JD_In' */
16990 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
16991 motor_B.JD_In = motor_U.JD_In;
16992 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
16993
16994 /* Inport: '<Root>/Encode_Sp' */
16995 motor_B.Encode_Sp = motor_U.Encode_Sp;
16996
16997 /* Inport: '<Root>/System_Order' */
16998 motor_B.Slect_port = motor_U.System_Order;
16999
17000 /* Inport: '<Root>/SGWY_In' */
17001 motor_B.SGWY_In = motor_U.SGWY_In;
17002
17003 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17004 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17005 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17006 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17007 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17008 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17009 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17010 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17011 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17012 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17013 &motor_DWork.chu_jd);
17014
17015 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17016 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17017 if (tmp_0 < 65536.0) {
17018 if (tmp_0 >= 0.0) {
17019 tmp_1 = (uint16_T)tmp_0;
17020 } else {
17021 tmp_1 = 0U;
17022 }
17023 } else {
17024 tmp_1 = MAX_uint16_T;
17025 }
17026
17027 motor_Y.PWMOUT = tmp_1;
17028 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17029 if (tmp_0 < 2.147483648E+9) {
17030 if (tmp_0 >= -2.147483648E+9) {
17031 tmp = (int32_T)tmp_0;
17032 } else {
17033 tmp = MIN_int32_T;
17034 }
17035 } else {
17036 tmp = MAX_int32_T;
17037 }
17038
17039 motor_Y.JD_Out = tmp;
17040 tmp_0 = motor_B.Motor_XHHY.KP_e;
17041 if (tmp_0 < 2.147483648E+9) {
17042 if (tmp_0 >= -2.147483648E+9) {
17043 tmp = (int32_T)tmp_0;
17044 } else {
17045 tmp = MIN_int32_T;
17046 }
17047 } else {
17048 tmp = MAX_int32_T;
17049 }
17050
17051 motor_Y.JD_Error = tmp;
17052 } else if (motor_DWork.temporalCounter_i2 >= (uint32_T)(5.0 /
17053 motor_DWork.Ts)) {
17054 /* Transition: '<S1>:1073' */
17055 motor_Y.Flag_Down_GZ_limit = 0U;
17056
17057 /* 下限位开关故障$/ */
17058 motor_DWork.is_XHHY_fs = motor_IN_Int2_i;
17059 motor_DWork.temporalCounter_i2 = 0U;
17060
17061 /* Entry 'Int2': '<S1>:335' */
17062 motor_DWork.chu_jd -= 0.002;
17063
17064 /* Inport: '<Root>/JD_In' */
17065 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17066 motor_B.JD_In = motor_U.JD_In;
17067 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17068
17069 /* Inport: '<Root>/Encode_Sp' */
17070 motor_B.Encode_Sp = motor_U.Encode_Sp;
17071
17072 /* Inport: '<Root>/System_Order' */
17073 motor_B.Slect_port = motor_U.System_Order;
17074
17075 /* Inport: '<Root>/SGWY_In' */
17076 motor_B.SGWY_In = motor_U.SGWY_In;
17077
17078 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17079 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17080 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17081 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17082 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17083 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17084 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17085 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17086 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17087 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17088 &motor_DWork.chu_jd);
17089
17090 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17091 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17092 if (tmp_0 < 65536.0) {
17093 if (tmp_0 >= 0.0) {
17094 tmp_1 = (uint16_T)tmp_0;
17095 } else {
17096 tmp_1 = 0U;
17097 }
17098 } else {
17099 tmp_1 = MAX_uint16_T;
17100 }
17101
17102 motor_Y.PWMOUT = tmp_1;
17103 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17104 if (tmp_0 < 2.147483648E+9) {
17105 if (tmp_0 >= -2.147483648E+9) {
17106 tmp = (int32_T)tmp_0;
17107 } else {
17108 tmp = MIN_int32_T;
17109 }
17110 } else {
17111 tmp = MAX_int32_T;
17112 }
17113
17114 motor_Y.JD_Out = tmp;
17115 tmp_0 = motor_B.Motor_XHHY.KP_e;
17116 if (tmp_0 < 2.147483648E+9) {
17117 if (tmp_0 >= -2.147483648E+9) {
17118 tmp = (int32_T)tmp_0;
17119 } else {
17120 tmp = MIN_int32_T;
17121 }
17122 } else {
17123 tmp = MAX_int32_T;
17124 }
17125
17126 motor_Y.JD_Error = tmp;
17127 } else {
17128 motor_DWork.chu_jd -= 0.002;
17129
17130 /* Inport: '<Root>/JD_In' */
17131 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17132 motor_B.JD_In = motor_U.JD_In;
17133 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17134
17135 /* Inport: '<Root>/Encode_Sp' */
17136 motor_B.Encode_Sp = motor_U.Encode_Sp;
17137
17138 /* Inport: '<Root>/System_Order' */
17139 motor_B.Slect_port = motor_U.System_Order;
17140
17141 /* Inport: '<Root>/SGWY_In' */
17142 motor_B.SGWY_In = motor_U.SGWY_In;
17143
17144 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17145 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17146 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17147 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17148 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17149 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17150 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17151 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17152 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17153 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17154 &motor_DWork.chu_jd);
17155
17156 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17157 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17158 if (tmp_0 < 65536.0) {
17159 if (tmp_0 >= 0.0) {
17160 tmp_1 = (uint16_T)tmp_0;
17161 } else {
17162 tmp_1 = 0U;
17163 }
17164 } else {
17165 tmp_1 = MAX_uint16_T;
17166 }
17167
17168 motor_Y.PWMOUT = tmp_1;
17169 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17170 if (tmp_0 < 2.147483648E+9) {
17171 if (tmp_0 >= -2.147483648E+9) {
17172 tmp = (int32_T)tmp_0;
17173 } else {
17174 tmp = MIN_int32_T;
17175 }
17176 } else {
17177 tmp = MAX_int32_T;
17178 }
17179
17180 motor_Y.JD_Out = tmp;
17181 tmp_0 = motor_B.Motor_XHHY.KP_e;
17182 if (tmp_0 < 2.147483648E+9) {
17183 if (tmp_0 >= -2.147483648E+9) {
17184 tmp = (int32_T)tmp_0;
17185 } else {
17186 tmp = MIN_int32_T;
17187 }
17188 } else {
17189 tmp = MAX_int32_T;
17190 }
17191
17192 motor_Y.JD_Error = tmp;
17193 }
17194 break;
17195
17196 case motor_IN_Int2_i:
17197 /* During 'Int2': '<S1>:335' */
17198 if (motor_DWork.temporalCounter_i2 >= 50U) {
17199 /* Transition: '<S1>:769' */
17200 motor_DWork.is_XHHY_fs = motor_IN_Int7;
17201 motor_DWork.temporalCounter_i2 = 0U;
17202 } else {
17203 motor_DWork.chu_jd -= 0.002;
17204
17205 /* Inport: '<Root>/JD_In' */
17206 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17207 motor_B.JD_In = motor_U.JD_In;
17208 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17209
17210 /* Inport: '<Root>/Encode_Sp' */
17211 motor_B.Encode_Sp = motor_U.Encode_Sp;
17212
17213 /* Inport: '<Root>/System_Order' */
17214 motor_B.Slect_port = motor_U.System_Order;
17215
17216 /* Inport: '<Root>/SGWY_In' */
17217 motor_B.SGWY_In = motor_U.SGWY_In;
17218
17219 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17220 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17221 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17222 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17223 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17224 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17225 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17226 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17227 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17228 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17229 &motor_DWork.chu_jd);
17230
17231 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17232 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17233 if (tmp_0 < 65536.0) {
17234 if (tmp_0 >= 0.0) {
17235 tmp_1 = (uint16_T)tmp_0;
17236 } else {
17237 tmp_1 = 0U;
17238 }
17239 } else {
17240 tmp_1 = MAX_uint16_T;
17241 }
17242
17243 motor_Y.PWMOUT = tmp_1;
17244 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17245 if (tmp_0 < 2.147483648E+9) {
17246 if (tmp_0 >= -2.147483648E+9) {
17247 tmp = (int32_T)tmp_0;
17248 } else {
17249 tmp = MIN_int32_T;
17250 }
17251 } else {
17252 tmp = MAX_int32_T;
17253 }
17254
17255 motor_Y.JD_Out = tmp;
17256 tmp_0 = motor_B.Motor_XHHY.KP_e;
17257 if (tmp_0 < 2.147483648E+9) {
17258 if (tmp_0 >= -2.147483648E+9) {
17259 tmp = (int32_T)tmp_0;
17260 } else {
17261 tmp = MIN_int32_T;
17262 }
17263 } else {
17264 tmp = MAX_int32_T;
17265 }
17266
17267 motor_Y.JD_Error = tmp;
17268 }
17269 break;
17270
17271 case motor_IN_Int3_l:
17272 /* During 'Int3': '<S1>:336' */
17273 if (fabs(motor_DWork.chu_jd) < 0.1) {
17274 /* Transition: '<S1>:770' */
17275 motor_DWork.is_XHHY_fs = motor_IN_Int4_b;
17276 motor_DWork.temporalCounter_i2 = 0U;
17277
17278 /* Entry 'Int4': '<S1>:338' */
17279 motor_DWork.chu_jd = 0.0;
17280
17281 /* Inport: '<Root>/JD_In' */
17282 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17283 motor_B.JD_In = motor_U.JD_In;
17284 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17285
17286 /* Inport: '<Root>/Encode_Sp' */
17287 motor_B.Encode_Sp = motor_U.Encode_Sp;
17288
17289 /* Inport: '<Root>/System_Order' */
17290 motor_B.Slect_port = motor_U.System_Order;
17291
17292 /* Inport: '<Root>/SGWY_In' */
17293 motor_B.SGWY_In = motor_U.SGWY_In;
17294
17295 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17296 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17297 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17298 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17299 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17300 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17301 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17302 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17303 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17304 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17305 &motor_DWork.chu_jd);
17306
17307 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17308 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17309 if (tmp_0 < 65536.0) {
17310 if (tmp_0 >= 0.0) {
17311 tmp_1 = (uint16_T)tmp_0;
17312 } else {
17313 tmp_1 = 0U;
17314 }
17315 } else {
17316 tmp_1 = MAX_uint16_T;
17317 }
17318
17319 motor_Y.PWMOUT = tmp_1;
17320 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17321 if (tmp_0 < 2.147483648E+9) {
17322 if (tmp_0 >= -2.147483648E+9) {
17323 tmp = (int32_T)tmp_0;
17324 } else {
17325 tmp = MIN_int32_T;
17326 }
17327 } else {
17328 tmp = MAX_int32_T;
17329 }
17330
17331 motor_Y.JD_Out = tmp;
17332 tmp_0 = motor_B.Motor_XHHY.KP_e;
17333 if (tmp_0 < 2.147483648E+9) {
17334 if (tmp_0 >= -2.147483648E+9) {
17335 tmp = (int32_T)tmp_0;
17336 } else {
17337 tmp = MIN_int32_T;
17338 }
17339 } else {
17340 tmp = MAX_int32_T;
17341 }
17342
17343 motor_Y.JD_Error = tmp;
17344 } else {
17345 motor_DWork.chu_jd += 0.01;
17346
17347 /* Inport: '<Root>/JD_In' */
17348 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17349 motor_B.JD_In = motor_U.JD_In;
17350 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17351
17352 /* Inport: '<Root>/Encode_Sp' */
17353 motor_B.Encode_Sp = motor_U.Encode_Sp;
17354
17355 /* Inport: '<Root>/System_Order' */
17356 motor_B.Slect_port = motor_U.System_Order;
17357
17358 /* Inport: '<Root>/SGWY_In' */
17359 motor_B.SGWY_In = motor_U.SGWY_In;
17360
17361 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17362 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17363 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17364 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17365 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17366 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17367 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17368 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17369 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17370 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17371 &motor_DWork.chu_jd);
17372
17373 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17374 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17375 if (tmp_0 < 65536.0) {
17376 if (tmp_0 >= 0.0) {
17377 tmp_1 = (uint16_T)tmp_0;
17378 } else {
17379 tmp_1 = 0U;
17380 }
17381 } else {
17382 tmp_1 = MAX_uint16_T;
17383 }
17384
17385 motor_Y.PWMOUT = tmp_1;
17386 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17387 if (tmp_0 < 2.147483648E+9) {
17388 if (tmp_0 >= -2.147483648E+9) {
17389 tmp = (int32_T)tmp_0;
17390 } else {
17391 tmp = MIN_int32_T;
17392 }
17393 } else {
17394 tmp = MAX_int32_T;
17395 }
17396
17397 motor_Y.JD_Out = tmp;
17398 tmp_0 = motor_B.Motor_XHHY.KP_e;
17399 if (tmp_0 < 2.147483648E+9) {
17400 if (tmp_0 >= -2.147483648E+9) {
17401 tmp = (int32_T)tmp_0;
17402 } else {
17403 tmp = MIN_int32_T;
17404 }
17405 } else {
17406 tmp = MAX_int32_T;
17407 }
17408
17409 motor_Y.JD_Error = tmp;
17410 }
17411 break;
17412
17413 case motor_IN_Int4_b:
17414 /* During 'Int4': '<S1>:338' */
17415 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(2.0 / motor_DWork.Ts)) {
17416 /* Transition: '<S1>:772' */
17417 motor_DWork.is_XHHY_fs = motor_IN_Int5_a;
17418 motor_DWork.temporalCounter_i2 = 0U;
17419
17420 /* Entry 'Int5': '<S1>:339' */
17421 motor_Y.Motor_En = true;
17422
17423 /* 电机失能 */
17424 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
17425 } else {
17426 /* Inport: '<Root>/JD_In' */
17427 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17428 motor_B.JD_In = motor_U.JD_In;
17429 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17430
17431 /* Inport: '<Root>/Encode_Sp' */
17432 motor_B.Encode_Sp = motor_U.Encode_Sp;
17433
17434 /* Inport: '<Root>/System_Order' */
17435 motor_B.Slect_port = motor_U.System_Order;
17436
17437 /* Inport: '<Root>/SGWY_In' */
17438 motor_B.SGWY_In = motor_U.SGWY_In;
17439
17440 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17441 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17442 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17443 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17444 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17445 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17446 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17447 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17448 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17449 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17450 &motor_DWork.chu_jd);
17451
17452 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17453 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17454 if (tmp_0 < 65536.0) {
17455 if (tmp_0 >= 0.0) {
17456 tmp_1 = (uint16_T)tmp_0;
17457 } else {
17458 tmp_1 = 0U;
17459 }
17460 } else {
17461 tmp_1 = MAX_uint16_T;
17462 }
17463
17464 motor_Y.PWMOUT = tmp_1;
17465 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17466 if (tmp_0 < 2.147483648E+9) {
17467 if (tmp_0 >= -2.147483648E+9) {
17468 tmp = (int32_T)tmp_0;
17469 } else {
17470 tmp = MIN_int32_T;
17471 }
17472 } else {
17473 tmp = MAX_int32_T;
17474 }
17475
17476 motor_Y.JD_Out = tmp;
17477 tmp_0 = motor_B.Motor_XHHY.KP_e;
17478 if (tmp_0 < 2.147483648E+9) {
17479 if (tmp_0 >= -2.147483648E+9) {
17480 tmp = (int32_T)tmp_0;
17481 } else {
17482 tmp = MIN_int32_T;
17483 }
17484 } else {
17485 tmp = MAX_int32_T;
17486 }
17487
17488 motor_Y.JD_Error = tmp;
17489 }
17490 break;
17491
17492 case motor_IN_Int5_a:
17493 /* During 'Int5': '<S1>:339' */
17494 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
17495 /* Transition: '<S1>:771' */
17496 motor_DWork.is_XHHY_fs = motor_IN_Int6_m;
17497
17498 /* Entry 'Int6': '<S1>:337' */
17499 motor_Y.DCZD = true;
17500
17501 /* 开启制动 */
17502 }
17503 break;
17504
17505 case motor_IN_Int6_m:
17506 /* During 'Int6': '<S1>:337' */
17507 /* Transition: '<S1>:768' */
17508 /* Transition: '<S1>:738' */
17509 /* Transition: '<S1>:620' */
17510 motor_DWork.XiaoDaShen = 0.0;
17511 motor_DWork.is_XHHY_fs = motor_IN_NO_ACTIVE_CHILD;
17512 motor_DWork.is_Limit_Down_Test_i = motor_IN_NO_ACTIVE_CHILD;
17513 motor_DWork.is_Algorithm_e = motor_IN_Defualt;
17514
17515 /* Entry 'Defualt': '<S1>:206' */
17516 motor_DWork.chu_jd = 0.0;
17517 motor_DWork.KG = 0U;
17518 motor_Y.Motor_En = false;
17519 motor_Y.DCZD = false;
17520
17521 /* 开电磁制动,低有效 */
17522 break;
17523
17524 case motor_IN_Int7:
17525 /* During 'Int7': '<S1>:965' */
17526 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
17527 /* Transition: '<S1>:966' */
17528 motor_DWork.is_XHHY_fs = motor_IN_Int3_l;
17529
17530 /* Entry 'Int3': '<S1>:336' */
17531 motor_DWork.chu_jd += 0.01;
17532
17533 /* Inport: '<Root>/JD_In' */
17534 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17535 motor_B.JD_In = motor_U.JD_In;
17536 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17537
17538 /* Inport: '<Root>/Encode_Sp' */
17539 motor_B.Encode_Sp = motor_U.Encode_Sp;
17540
17541 /* Inport: '<Root>/System_Order' */
17542 motor_B.Slect_port = motor_U.System_Order;
17543
17544 /* Inport: '<Root>/SGWY_In' */
17545 motor_B.SGWY_In = motor_U.SGWY_In;
17546
17547 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17548 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17549 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17550 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17551 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17552 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17553 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17554 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17555 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17556 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17557 &motor_DWork.chu_jd);
17558
17559 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17560 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17561 if (tmp_0 < 65536.0) {
17562 if (tmp_0 >= 0.0) {
17563 tmp_1 = (uint16_T)tmp_0;
17564 } else {
17565 tmp_1 = 0U;
17566 }
17567 } else {
17568 tmp_1 = MAX_uint16_T;
17569 }
17570
17571 motor_Y.PWMOUT = tmp_1;
17572 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17573 if (tmp_0 < 2.147483648E+9) {
17574 if (tmp_0 >= -2.147483648E+9) {
17575 tmp = (int32_T)tmp_0;
17576 } else {
17577 tmp = MIN_int32_T;
17578 }
17579 } else {
17580 tmp = MAX_int32_T;
17581 }
17582
17583 motor_Y.JD_Out = tmp;
17584 tmp_0 = motor_B.Motor_XHHY.KP_e;
17585 if (tmp_0 < 2.147483648E+9) {
17586 if (tmp_0 >= -2.147483648E+9) {
17587 tmp = (int32_T)tmp_0;
17588 } else {
17589 tmp = MIN_int32_T;
17590 }
17591 } else {
17592 tmp = MAX_int32_T;
17593 }
17594
17595 motor_Y.JD_Error = tmp;
17596 } else {
17597 /* Inport: '<Root>/JD_In' */
17598 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17599 motor_B.JD_In = motor_U.JD_In;
17600 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17601
17602 /* Inport: '<Root>/Encode_Sp' */
17603 motor_B.Encode_Sp = motor_U.Encode_Sp;
17604
17605 /* Inport: '<Root>/System_Order' */
17606 motor_B.Slect_port = motor_U.System_Order;
17607
17608 /* Inport: '<Root>/SGWY_In' */
17609 motor_B.SGWY_In = motor_U.SGWY_In;
17610
17611 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17612 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17613 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17614 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17615 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17616 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17617 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17618 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17619 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17620 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17621 &motor_DWork.chu_jd);
17622
17623 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17624 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17625 if (tmp_0 < 65536.0) {
17626 if (tmp_0 >= 0.0) {
17627 tmp_1 = (uint16_T)tmp_0;
17628 } else {
17629 tmp_1 = 0U;
17630 }
17631 } else {
17632 tmp_1 = MAX_uint16_T;
17633 }
17634
17635 motor_Y.PWMOUT = tmp_1;
17636 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17637 if (tmp_0 < 2.147483648E+9) {
17638 if (tmp_0 >= -2.147483648E+9) {
17639 tmp = (int32_T)tmp_0;
17640 } else {
17641 tmp = MIN_int32_T;
17642 }
17643 } else {
17644 tmp = MAX_int32_T;
17645 }
17646
17647 motor_Y.JD_Out = tmp;
17648 tmp_0 = motor_B.Motor_XHHY.KP_e;
17649 if (tmp_0 < 2.147483648E+9) {
17650 if (tmp_0 >= -2.147483648E+9) {
17651 tmp = (int32_T)tmp_0;
17652 } else {
17653 tmp = MIN_int32_T;
17654 }
17655 } else {
17656 tmp = MAX_int32_T;
17657 }
17658
17659 motor_Y.JD_Error = tmp;
17660 }
17661 break;
17662
17663 default:
17664 /* During 'Int8': '<S1>:1026' */
17665 if (motor_DWork.chu_jd < -16.0) {
17666 /* Transition: '<S1>:1027' */
17667 motor_DWork.is_XHHY_fs = motor_IN_Int1;
17668 motor_DWork.temporalCounter_i2 = 0U;
17669
17670 /* Entry 'Int1': '<S1>:334' */
17671 motor_DWork.chu_jd -= 0.002;
17672
17673 /* Inport: '<Root>/JD_In' */
17674 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17675 motor_B.JD_In = motor_U.JD_In;
17676 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17677
17678 /* Inport: '<Root>/Encode_Sp' */
17679 motor_B.Encode_Sp = motor_U.Encode_Sp;
17680
17681 /* Inport: '<Root>/System_Order' */
17682 motor_B.Slect_port = motor_U.System_Order;
17683
17684 /* Inport: '<Root>/SGWY_In' */
17685 motor_B.SGWY_In = motor_U.SGWY_In;
17686
17687 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17688 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17689 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17690 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17691 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17692 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17693 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17694 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17695 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17696 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17697 &motor_DWork.chu_jd);
17698
17699 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17700 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17701 if (tmp_0 < 65536.0) {
17702 if (tmp_0 >= 0.0) {
17703 tmp_1 = (uint16_T)tmp_0;
17704 } else {
17705 tmp_1 = 0U;
17706 }
17707 } else {
17708 tmp_1 = MAX_uint16_T;
17709 }
17710
17711 motor_Y.PWMOUT = tmp_1;
17712 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17713 if (tmp_0 < 2.147483648E+9) {
17714 if (tmp_0 >= -2.147483648E+9) {
17715 tmp = (int32_T)tmp_0;
17716 } else {
17717 tmp = MIN_int32_T;
17718 }
17719 } else {
17720 tmp = MAX_int32_T;
17721 }
17722
17723 motor_Y.JD_Out = tmp;
17724 tmp_0 = motor_B.Motor_XHHY.KP_e;
17725 if (tmp_0 < 2.147483648E+9) {
17726 if (tmp_0 >= -2.147483648E+9) {
17727 tmp = (int32_T)tmp_0;
17728 } else {
17729 tmp = MIN_int32_T;
17730 }
17731 } else {
17732 tmp = MAX_int32_T;
17733 }
17734
17735 motor_Y.JD_Error = tmp;
17736 } else if (motor_U.Down_Limit == 0) {
17737 /* Transition: '<S1>:1028' */
17738 /* 下限位开关低电平有效 */
17739 motor_DWork.is_XHHY_fs = motor_IN_Int2_i;
17740 motor_DWork.temporalCounter_i2 = 0U;
17741
17742 /* Entry 'Int2': '<S1>:335' */
17743 motor_DWork.chu_jd -= 0.002;
17744
17745 /* Inport: '<Root>/JD_In' */
17746 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17747 motor_B.JD_In = motor_U.JD_In;
17748 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17749
17750 /* Inport: '<Root>/Encode_Sp' */
17751 motor_B.Encode_Sp = motor_U.Encode_Sp;
17752
17753 /* Inport: '<Root>/System_Order' */
17754 motor_B.Slect_port = motor_U.System_Order;
17755
17756 /* Inport: '<Root>/SGWY_In' */
17757 motor_B.SGWY_In = motor_U.SGWY_In;
17758
17759 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17760 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17761 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17762 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17763 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17764 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17765 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17766 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17767 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17768 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17769 &motor_DWork.chu_jd);
17770
17771 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17772 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17773 if (tmp_0 < 65536.0) {
17774 if (tmp_0 >= 0.0) {
17775 tmp_1 = (uint16_T)tmp_0;
17776 } else {
17777 tmp_1 = 0U;
17778 }
17779 } else {
17780 tmp_1 = MAX_uint16_T;
17781 }
17782
17783 motor_Y.PWMOUT = tmp_1;
17784 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17785 if (tmp_0 < 2.147483648E+9) {
17786 if (tmp_0 >= -2.147483648E+9) {
17787 tmp = (int32_T)tmp_0;
17788 } else {
17789 tmp = MIN_int32_T;
17790 }
17791 } else {
17792 tmp = MAX_int32_T;
17793 }
17794
17795 motor_Y.JD_Out = tmp;
17796 tmp_0 = motor_B.Motor_XHHY.KP_e;
17797 if (tmp_0 < 2.147483648E+9) {
17798 if (tmp_0 >= -2.147483648E+9) {
17799 tmp = (int32_T)tmp_0;
17800 } else {
17801 tmp = MIN_int32_T;
17802 }
17803 } else {
17804 tmp = MAX_int32_T;
17805 }
17806
17807 motor_Y.JD_Error = tmp;
17808 } else {
17809 motor_DWork.chu_jd -= 0.01;
17810
17811 /* Inport: '<Root>/JD_In' */
17812 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
17813 motor_B.JD_In = motor_U.JD_In;
17814 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
17815
17816 /* Inport: '<Root>/Encode_Sp' */
17817 motor_B.Encode_Sp = motor_U.Encode_Sp;
17818
17819 /* Inport: '<Root>/System_Order' */
17820 motor_B.Slect_port = motor_U.System_Order;
17821
17822 /* Inport: '<Root>/SGWY_In' */
17823 motor_B.SGWY_In = motor_U.SGWY_In;
17824
17825 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
17826 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
17827 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
17828 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
17829 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
17830 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17831 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
17832 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
17833 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
17834 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17835 &motor_DWork.chu_jd);
17836
17837 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
17838 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
17839 if (tmp_0 < 65536.0) {
17840 if (tmp_0 >= 0.0) {
17841 tmp_1 = (uint16_T)tmp_0;
17842 } else {
17843 tmp_1 = 0U;
17844 }
17845 } else {
17846 tmp_1 = MAX_uint16_T;
17847 }
17848
17849 motor_Y.PWMOUT = tmp_1;
17850 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
17851 if (tmp_0 < 2.147483648E+9) {
17852 if (tmp_0 >= -2.147483648E+9) {
17853 tmp = (int32_T)tmp_0;
17854 } else {
17855 tmp = MIN_int32_T;
17856 }
17857 } else {
17858 tmp = MAX_int32_T;
17859 }
17860
17861 motor_Y.JD_Out = tmp;
17862 tmp_0 = motor_B.Motor_XHHY.KP_e;
17863 if (tmp_0 < 2.147483648E+9) {
17864 if (tmp_0 >= -2.147483648E+9) {
17865 tmp = (int32_T)tmp_0;
17866 } else {
17867 tmp = MIN_int32_T;
17868 }
17869 } else {
17870 tmp = MAX_int32_T;
17871 }
17872
17873 motor_Y.JD_Error = tmp;
17874 }
17875 break;
17876 }
17877 break;
17878
17879 default:
17880 motor_XHZY_asn();
17881 break;
17882 }
17883}
17884
17885/* Function for Chart: '<Root>/Chart' */
17886static void motor_XHZY_asnq(void)
17887{
17888 int32_T tmp;
17889 uint16_T tmp_0;
17890 real_T tmp_1;
17891
17892 /* During 'XHZY': '<S1>:341' */
17893 switch (motor_DWork.is_XHZY_d) {
17894 case motor_IN_Int:
17895 /* During 'Int': '<S1>:342' */
17896 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
17897 /* Transition: '<S1>:1005' */
17898 motor_DWork.is_XHZY_d = motor_IN_Int8;
17899 }
17900 break;
17901
17902 case motor_IN_Int1:
17903 /* Inport: '<Root>/Up_Limit' */
17904 /* During 'Int1': '<S1>:343' */
17905 if (motor_U.Up_Limit == 0) {
17906 /* Transition: '<S1>:786' */
17907 /* 上限位开关低电平有效 */
17908 motor_DWork.is_XHZY_d = motor_IN_Int2_i;
17909 motor_DWork.temporalCounter_i2 = 0U;
17910
17911 /* Entry 'Int2': '<S1>:344' */
17912 motor_DWork.chu_jd += 0.002;
17913
17914 /* Inport: '<Root>/JD_In' */
17915 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
17916 motor_B.JD_In_d = motor_U.JD_In;
17917 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
17918
17919 /* Inport: '<Root>/YJ_In' */
17920 motor_B.YJ_In = motor_U.YJ_In;
17921
17922 /* Inport: '<Root>/Encode_Sp' */
17923 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
17924
17925 /* Inport: '<Root>/System_Order' */
17926 motor_B.Slect_port_h = motor_U.System_Order;
17927
17928 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
17929 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
17930 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
17931 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
17932 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
17933 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
17934 &motor_DWork.EN_extern, &motor_DWork.Forword,
17935 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
17936 &motor_DWork.KG_En, &motor_DWork.KG_JD,
17937 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
17938 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
17939 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
17940 &motor_DWork.chu_jd);
17941
17942 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
17943 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
17944 if (tmp_1 < 65536.0) {
17945 if (tmp_1 >= 0.0) {
17946 tmp_0 = (uint16_T)tmp_1;
17947 } else {
17948 tmp_0 = 0U;
17949 }
17950 } else {
17951 tmp_0 = MAX_uint16_T;
17952 }
17953
17954 motor_Y.PWMOUT = tmp_0;
17955 tmp_1 = motor_B.Motor_XHZY.KP_JD;
17956 if (tmp_1 < 2.147483648E+9) {
17957 if (tmp_1 >= -2.147483648E+9) {
17958 tmp = (int32_T)tmp_1;
17959 } else {
17960 tmp = MIN_int32_T;
17961 }
17962 } else {
17963 tmp = MAX_int32_T;
17964 }
17965
17966 motor_Y.JD_Out = tmp;
17967 tmp_1 = motor_B.Motor_XHZY.KP_e;
17968 if (tmp_1 < 2.147483648E+9) {
17969 if (tmp_1 >= -2.147483648E+9) {
17970 tmp = (int32_T)tmp_1;
17971 } else {
17972 tmp = MIN_int32_T;
17973 }
17974 } else {
17975 tmp = MAX_int32_T;
17976 }
17977
17978 motor_Y.JD_Error = tmp;
17979 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
17980 } else if (motor_DWork.temporalCounter_i2 >= (uint32_T)(5.0 / motor_DWork.Ts))
17981 {
17982 /* Transition: '<S1>:1042' */
17983 motor_Y.Flag_Up_GZ_limit = 0U;
17984
17985 /* 上限位开关故障$/ */
17986 motor_DWork.is_XHZY_d = motor_IN_Int2_i;
17987 motor_DWork.temporalCounter_i2 = 0U;
17988
17989 /* Entry 'Int2': '<S1>:344' */
17990 motor_DWork.chu_jd += 0.002;
17991
17992 /* Inport: '<Root>/JD_In' */
17993 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
17994 motor_B.JD_In_d = motor_U.JD_In;
17995 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
17996
17997 /* Inport: '<Root>/YJ_In' */
17998 motor_B.YJ_In = motor_U.YJ_In;
17999
18000 /* Inport: '<Root>/Encode_Sp' */
18001 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18002
18003 /* Inport: '<Root>/System_Order' */
18004 motor_B.Slect_port_h = motor_U.System_Order;
18005
18006 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18007 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18008 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18009 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18010 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18011 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18012 &motor_DWork.EN_extern, &motor_DWork.Forword,
18013 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18014 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18015 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18016 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18017 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18018 &motor_DWork.chu_jd);
18019
18020 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18021 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18022 if (tmp_1 < 65536.0) {
18023 if (tmp_1 >= 0.0) {
18024 tmp_0 = (uint16_T)tmp_1;
18025 } else {
18026 tmp_0 = 0U;
18027 }
18028 } else {
18029 tmp_0 = MAX_uint16_T;
18030 }
18031
18032 motor_Y.PWMOUT = tmp_0;
18033 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18034 if (tmp_1 < 2.147483648E+9) {
18035 if (tmp_1 >= -2.147483648E+9) {
18036 tmp = (int32_T)tmp_1;
18037 } else {
18038 tmp = MIN_int32_T;
18039 }
18040 } else {
18041 tmp = MAX_int32_T;
18042 }
18043
18044 motor_Y.JD_Out = tmp;
18045 tmp_1 = motor_B.Motor_XHZY.KP_e;
18046 if (tmp_1 < 2.147483648E+9) {
18047 if (tmp_1 >= -2.147483648E+9) {
18048 tmp = (int32_T)tmp_1;
18049 } else {
18050 tmp = MIN_int32_T;
18051 }
18052 } else {
18053 tmp = MAX_int32_T;
18054 }
18055
18056 motor_Y.JD_Error = tmp;
18057 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18058 } else {
18059 motor_DWork.chu_jd += 0.002;
18060
18061 /* Inport: '<Root>/JD_In' */
18062 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
18063 motor_B.JD_In_d = motor_U.JD_In;
18064 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
18065
18066 /* Inport: '<Root>/YJ_In' */
18067 motor_B.YJ_In = motor_U.YJ_In;
18068
18069 /* Inport: '<Root>/Encode_Sp' */
18070 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18071
18072 /* Inport: '<Root>/System_Order' */
18073 motor_B.Slect_port_h = motor_U.System_Order;
18074
18075 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18076 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18077 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18078 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18079 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18080 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18081 &motor_DWork.EN_extern, &motor_DWork.Forword,
18082 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18083 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18084 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18085 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18086 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18087 &motor_DWork.chu_jd);
18088
18089 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18090 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18091 if (tmp_1 < 65536.0) {
18092 if (tmp_1 >= 0.0) {
18093 tmp_0 = (uint16_T)tmp_1;
18094 } else {
18095 tmp_0 = 0U;
18096 }
18097 } else {
18098 tmp_0 = MAX_uint16_T;
18099 }
18100
18101 motor_Y.PWMOUT = tmp_0;
18102 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18103 if (tmp_1 < 2.147483648E+9) {
18104 if (tmp_1 >= -2.147483648E+9) {
18105 tmp = (int32_T)tmp_1;
18106 } else {
18107 tmp = MIN_int32_T;
18108 }
18109 } else {
18110 tmp = MAX_int32_T;
18111 }
18112
18113 motor_Y.JD_Out = tmp;
18114 tmp_1 = motor_B.Motor_XHZY.KP_e;
18115 if (tmp_1 < 2.147483648E+9) {
18116 if (tmp_1 >= -2.147483648E+9) {
18117 tmp = (int32_T)tmp_1;
18118 } else {
18119 tmp = MIN_int32_T;
18120 }
18121 } else {
18122 tmp = MAX_int32_T;
18123 }
18124
18125 motor_Y.JD_Error = tmp;
18126 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18127 }
18128
18129 /* End of Inport: '<Root>/Up_Limit' */
18130 break;
18131
18132 case motor_IN_Int2_i:
18133 /* During 'Int2': '<S1>:344' */
18134 if (motor_DWork.temporalCounter_i2 >= 50U) {
18135 /* Transition: '<S1>:788' */
18136 motor_DWork.is_XHZY_d = motor_IN_Int7;
18137 motor_DWork.temporalCounter_i2 = 0U;
18138 } else {
18139 motor_DWork.chu_jd += 0.002;
18140
18141 /* Inport: '<Root>/JD_In' */
18142 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
18143 motor_B.JD_In_d = motor_U.JD_In;
18144 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
18145
18146 /* Inport: '<Root>/YJ_In' */
18147 motor_B.YJ_In = motor_U.YJ_In;
18148
18149 /* Inport: '<Root>/Encode_Sp' */
18150 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18151
18152 /* Inport: '<Root>/System_Order' */
18153 motor_B.Slect_port_h = motor_U.System_Order;
18154
18155 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18156 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18157 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18158 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18159 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18160 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18161 &motor_DWork.EN_extern, &motor_DWork.Forword,
18162 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18163 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18164 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18165 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18166 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18167 &motor_DWork.chu_jd);
18168
18169 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18170 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18171 if (tmp_1 < 65536.0) {
18172 if (tmp_1 >= 0.0) {
18173 tmp_0 = (uint16_T)tmp_1;
18174 } else {
18175 tmp_0 = 0U;
18176 }
18177 } else {
18178 tmp_0 = MAX_uint16_T;
18179 }
18180
18181 motor_Y.PWMOUT = tmp_0;
18182 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18183 if (tmp_1 < 2.147483648E+9) {
18184 if (tmp_1 >= -2.147483648E+9) {
18185 tmp = (int32_T)tmp_1;
18186 } else {
18187 tmp = MIN_int32_T;
18188 }
18189 } else {
18190 tmp = MAX_int32_T;
18191 }
18192
18193 motor_Y.JD_Out = tmp;
18194 tmp_1 = motor_B.Motor_XHZY.KP_e;
18195 if (tmp_1 < 2.147483648E+9) {
18196 if (tmp_1 >= -2.147483648E+9) {
18197 tmp = (int32_T)tmp_1;
18198 } else {
18199 tmp = MIN_int32_T;
18200 }
18201 } else {
18202 tmp = MAX_int32_T;
18203 }
18204
18205 motor_Y.JD_Error = tmp;
18206 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18207 }
18208 break;
18209
18210 case motor_IN_Int3_l:
18211 /* During 'Int3': '<S1>:345' */
18212 if (fabs(motor_DWork.chu_jd) < 0.1) {
18213 /* Transition: '<S1>:789' */
18214 motor_DWork.is_XHZY_d = motor_IN_Int4_b;
18215 motor_DWork.temporalCounter_i2 = 0U;
18216
18217 /* Entry 'Int4': '<S1>:347' */
18218 motor_DWork.chu_jd = 0.0;
18219
18220 /* Inport: '<Root>/JD_In' */
18221 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
18222 motor_B.JD_In_d = motor_U.JD_In;
18223 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
18224
18225 /* Inport: '<Root>/YJ_In' */
18226 motor_B.YJ_In = motor_U.YJ_In;
18227
18228 /* Inport: '<Root>/Encode_Sp' */
18229 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18230
18231 /* Inport: '<Root>/System_Order' */
18232 motor_B.Slect_port_h = motor_U.System_Order;
18233
18234 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18235 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18236 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18237 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18238 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18239 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18240 &motor_DWork.EN_extern, &motor_DWork.Forword,
18241 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18242 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18243 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18244 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18245 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18246 &motor_DWork.chu_jd);
18247
18248 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18249 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18250 if (tmp_1 < 65536.0) {
18251 if (tmp_1 >= 0.0) {
18252 tmp_0 = (uint16_T)tmp_1;
18253 } else {
18254 tmp_0 = 0U;
18255 }
18256 } else {
18257 tmp_0 = MAX_uint16_T;
18258 }
18259
18260 motor_Y.PWMOUT = tmp_0;
18261 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18262 if (tmp_1 < 2.147483648E+9) {
18263 if (tmp_1 >= -2.147483648E+9) {
18264 tmp = (int32_T)tmp_1;
18265 } else {
18266 tmp = MIN_int32_T;
18267 }
18268 } else {
18269 tmp = MAX_int32_T;
18270 }
18271
18272 motor_Y.JD_Out = tmp;
18273 tmp_1 = motor_B.Motor_XHZY.KP_e;
18274 if (tmp_1 < 2.147483648E+9) {
18275 if (tmp_1 >= -2.147483648E+9) {
18276 tmp = (int32_T)tmp_1;
18277 } else {
18278 tmp = MIN_int32_T;
18279 }
18280 } else {
18281 tmp = MAX_int32_T;
18282 }
18283
18284 motor_Y.JD_Error = tmp;
18285 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18286 } else {
18287 motor_DWork.chu_jd -= 0.01;
18288
18289 /* Inport: '<Root>/JD_In' */
18290 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
18291 motor_B.JD_In_d = motor_U.JD_In;
18292 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
18293
18294 /* Inport: '<Root>/YJ_In' */
18295 motor_B.YJ_In = motor_U.YJ_In;
18296
18297 /* Inport: '<Root>/Encode_Sp' */
18298 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18299
18300 /* Inport: '<Root>/System_Order' */
18301 motor_B.Slect_port_h = motor_U.System_Order;
18302
18303 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18304 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18305 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18306 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18307 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18308 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18309 &motor_DWork.EN_extern, &motor_DWork.Forword,
18310 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18311 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18312 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18313 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18314 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18315 &motor_DWork.chu_jd);
18316
18317 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18318 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18319 if (tmp_1 < 65536.0) {
18320 if (tmp_1 >= 0.0) {
18321 tmp_0 = (uint16_T)tmp_1;
18322 } else {
18323 tmp_0 = 0U;
18324 }
18325 } else {
18326 tmp_0 = MAX_uint16_T;
18327 }
18328
18329 motor_Y.PWMOUT = tmp_0;
18330 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18331 if (tmp_1 < 2.147483648E+9) {
18332 if (tmp_1 >= -2.147483648E+9) {
18333 tmp = (int32_T)tmp_1;
18334 } else {
18335 tmp = MIN_int32_T;
18336 }
18337 } else {
18338 tmp = MAX_int32_T;
18339 }
18340
18341 motor_Y.JD_Out = tmp;
18342 tmp_1 = motor_B.Motor_XHZY.KP_e;
18343 if (tmp_1 < 2.147483648E+9) {
18344 if (tmp_1 >= -2.147483648E+9) {
18345 tmp = (int32_T)tmp_1;
18346 } else {
18347 tmp = MIN_int32_T;
18348 }
18349 } else {
18350 tmp = MAX_int32_T;
18351 }
18352
18353 motor_Y.JD_Error = tmp;
18354 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18355 }
18356 break;
18357
18358 case motor_IN_Int4_b:
18359 /* During 'Int4': '<S1>:347' */
18360 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(2.0 / motor_DWork.Ts)) {
18361 /* Transition: '<S1>:791' */
18362 motor_DWork.is_XHZY_d = motor_IN_Int5_a;
18363 motor_DWork.temporalCounter_i2 = 0U;
18364
18365 /* Entry 'Int5': '<S1>:348' */
18366 motor_Y.Motor_En = true;
18367
18368 /* 电机失能 */
18369 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
18370 } else {
18371 /* Inport: '<Root>/JD_In' */
18372 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
18373 motor_B.JD_In_d = motor_U.JD_In;
18374 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
18375
18376 /* Inport: '<Root>/YJ_In' */
18377 motor_B.YJ_In = motor_U.YJ_In;
18378
18379 /* Inport: '<Root>/Encode_Sp' */
18380 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18381
18382 /* Inport: '<Root>/System_Order' */
18383 motor_B.Slect_port_h = motor_U.System_Order;
18384
18385 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18386 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18387 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18388 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18389 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18390 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18391 &motor_DWork.EN_extern, &motor_DWork.Forword,
18392 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18393 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18394 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18395 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18396 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18397 &motor_DWork.chu_jd);
18398
18399 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18400 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18401 if (tmp_1 < 65536.0) {
18402 if (tmp_1 >= 0.0) {
18403 tmp_0 = (uint16_T)tmp_1;
18404 } else {
18405 tmp_0 = 0U;
18406 }
18407 } else {
18408 tmp_0 = MAX_uint16_T;
18409 }
18410
18411 motor_Y.PWMOUT = tmp_0;
18412 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18413 if (tmp_1 < 2.147483648E+9) {
18414 if (tmp_1 >= -2.147483648E+9) {
18415 tmp = (int32_T)tmp_1;
18416 } else {
18417 tmp = MIN_int32_T;
18418 }
18419 } else {
18420 tmp = MAX_int32_T;
18421 }
18422
18423 motor_Y.JD_Out = tmp;
18424 tmp_1 = motor_B.Motor_XHZY.KP_e;
18425 if (tmp_1 < 2.147483648E+9) {
18426 if (tmp_1 >= -2.147483648E+9) {
18427 tmp = (int32_T)tmp_1;
18428 } else {
18429 tmp = MIN_int32_T;
18430 }
18431 } else {
18432 tmp = MAX_int32_T;
18433 }
18434
18435 motor_Y.JD_Error = tmp;
18436 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18437 }
18438 break;
18439
18440 case motor_IN_Int5_a:
18441 /* During 'Int5': '<S1>:348' */
18442 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
18443 /* Transition: '<S1>:790' */
18444 motor_DWork.is_XHZY_d = motor_IN_Int6_m;
18445
18446 /* Entry 'Int6': '<S1>:346' */
18447 motor_Y.DCZD = true;
18448
18449 /* 开启制动 */
18450 }
18451 break;
18452
18453 case motor_IN_Int6_m:
18454 /* During 'Int6': '<S1>:346' */
18455 /* Transition: '<S1>:787' */
18456 /* Transition: '<S1>:773' */
18457 /* Transition: '<S1>:620' */
18458 motor_DWork.XiaoDaShen = 0.0;
18459 motor_DWork.is_XHZY_d = motor_IN_NO_ACTIVE_CHILD;
18460 motor_DWork.is_Limit_Up_Test_j = motor_IN_NO_ACTIVE_CHILD;
18461 motor_DWork.is_Algorithm_e = motor_IN_Defualt;
18462
18463 /* Entry 'Defualt': '<S1>:206' */
18464 motor_DWork.chu_jd = 0.0;
18465 motor_DWork.KG = 0U;
18466 motor_Y.Motor_En = false;
18467 motor_Y.DCZD = false;
18468
18469 /* 开电磁制动,低有效 */
18470 break;
18471
18472 case motor_IN_Int7:
18473 /* During 'Int7': '<S1>:955' */
18474 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
18475 /* Transition: '<S1>:956' */
18476 motor_DWork.is_XHZY_d = motor_IN_Int3_l;
18477
18478 /* Entry 'Int3': '<S1>:345' */
18479 motor_DWork.chu_jd -= 0.01;
18480
18481 /* Inport: '<Root>/JD_In' */
18482 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
18483 motor_B.JD_In_d = motor_U.JD_In;
18484 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
18485
18486 /* Inport: '<Root>/YJ_In' */
18487 motor_B.YJ_In = motor_U.YJ_In;
18488
18489 /* Inport: '<Root>/Encode_Sp' */
18490 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18491
18492 /* Inport: '<Root>/System_Order' */
18493 motor_B.Slect_port_h = motor_U.System_Order;
18494
18495 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18496 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18497 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18498 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18499 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18500 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18501 &motor_DWork.EN_extern, &motor_DWork.Forword,
18502 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18503 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18504 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18505 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18506 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18507 &motor_DWork.chu_jd);
18508
18509 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18510 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18511 if (tmp_1 < 65536.0) {
18512 if (tmp_1 >= 0.0) {
18513 tmp_0 = (uint16_T)tmp_1;
18514 } else {
18515 tmp_0 = 0U;
18516 }
18517 } else {
18518 tmp_0 = MAX_uint16_T;
18519 }
18520
18521 motor_Y.PWMOUT = tmp_0;
18522 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18523 if (tmp_1 < 2.147483648E+9) {
18524 if (tmp_1 >= -2.147483648E+9) {
18525 tmp = (int32_T)tmp_1;
18526 } else {
18527 tmp = MIN_int32_T;
18528 }
18529 } else {
18530 tmp = MAX_int32_T;
18531 }
18532
18533 motor_Y.JD_Out = tmp;
18534 tmp_1 = motor_B.Motor_XHZY.KP_e;
18535 if (tmp_1 < 2.147483648E+9) {
18536 if (tmp_1 >= -2.147483648E+9) {
18537 tmp = (int32_T)tmp_1;
18538 } else {
18539 tmp = MIN_int32_T;
18540 }
18541 } else {
18542 tmp = MAX_int32_T;
18543 }
18544
18545 motor_Y.JD_Error = tmp;
18546 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18547 } else {
18548 /* Inport: '<Root>/JD_In' */
18549 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
18550 motor_B.JD_In_d = motor_U.JD_In;
18551 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
18552
18553 /* Inport: '<Root>/YJ_In' */
18554 motor_B.YJ_In = motor_U.YJ_In;
18555
18556 /* Inport: '<Root>/Encode_Sp' */
18557 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18558
18559 /* Inport: '<Root>/System_Order' */
18560 motor_B.Slect_port_h = motor_U.System_Order;
18561
18562 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18563 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18564 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18565 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18566 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18567 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18568 &motor_DWork.EN_extern, &motor_DWork.Forword,
18569 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18570 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18571 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18572 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18573 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18574 &motor_DWork.chu_jd);
18575
18576 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18577 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18578 if (tmp_1 < 65536.0) {
18579 if (tmp_1 >= 0.0) {
18580 tmp_0 = (uint16_T)tmp_1;
18581 } else {
18582 tmp_0 = 0U;
18583 }
18584 } else {
18585 tmp_0 = MAX_uint16_T;
18586 }
18587
18588 motor_Y.PWMOUT = tmp_0;
18589 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18590 if (tmp_1 < 2.147483648E+9) {
18591 if (tmp_1 >= -2.147483648E+9) {
18592 tmp = (int32_T)tmp_1;
18593 } else {
18594 tmp = MIN_int32_T;
18595 }
18596 } else {
18597 tmp = MAX_int32_T;
18598 }
18599
18600 motor_Y.JD_Out = tmp;
18601 tmp_1 = motor_B.Motor_XHZY.KP_e;
18602 if (tmp_1 < 2.147483648E+9) {
18603 if (tmp_1 >= -2.147483648E+9) {
18604 tmp = (int32_T)tmp_1;
18605 } else {
18606 tmp = MIN_int32_T;
18607 }
18608 } else {
18609 tmp = MAX_int32_T;
18610 }
18611
18612 motor_Y.JD_Error = tmp;
18613 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18614 }
18615 break;
18616
18617 default:
18618 /* During 'Int8': '<S1>:1007' */
18619 if (motor_DWork.chu_jd > 16.0) {
18620 /* Transition: '<S1>:1008' */
18621 motor_DWork.is_XHZY_d = motor_IN_Int1;
18622 motor_DWork.temporalCounter_i2 = 0U;
18623
18624 /* Entry 'Int1': '<S1>:343' */
18625 motor_DWork.chu_jd += 0.002;
18626
18627 /* Inport: '<Root>/JD_In' */
18628 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
18629 motor_B.JD_In_d = motor_U.JD_In;
18630 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
18631
18632 /* Inport: '<Root>/YJ_In' */
18633 motor_B.YJ_In = motor_U.YJ_In;
18634
18635 /* Inport: '<Root>/Encode_Sp' */
18636 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18637
18638 /* Inport: '<Root>/System_Order' */
18639 motor_B.Slect_port_h = motor_U.System_Order;
18640
18641 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18642 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18643 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18644 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18645 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18646 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18647 &motor_DWork.EN_extern, &motor_DWork.Forword,
18648 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18649 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18650 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18651 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18652 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18653 &motor_DWork.chu_jd);
18654
18655 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18656 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18657 if (tmp_1 < 65536.0) {
18658 if (tmp_1 >= 0.0) {
18659 tmp_0 = (uint16_T)tmp_1;
18660 } else {
18661 tmp_0 = 0U;
18662 }
18663 } else {
18664 tmp_0 = MAX_uint16_T;
18665 }
18666
18667 motor_Y.PWMOUT = tmp_0;
18668 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18669 if (tmp_1 < 2.147483648E+9) {
18670 if (tmp_1 >= -2.147483648E+9) {
18671 tmp = (int32_T)tmp_1;
18672 } else {
18673 tmp = MIN_int32_T;
18674 }
18675 } else {
18676 tmp = MAX_int32_T;
18677 }
18678
18679 motor_Y.JD_Out = tmp;
18680 tmp_1 = motor_B.Motor_XHZY.KP_e;
18681 if (tmp_1 < 2.147483648E+9) {
18682 if (tmp_1 >= -2.147483648E+9) {
18683 tmp = (int32_T)tmp_1;
18684 } else {
18685 tmp = MIN_int32_T;
18686 }
18687 } else {
18688 tmp = MAX_int32_T;
18689 }
18690
18691 motor_Y.JD_Error = tmp;
18692 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18693 } else if (motor_U.Up_Limit == 0) {
18694 /* Transition: '<S1>:1006' */
18695 /* 上限位开关低电平有效 */
18696 motor_DWork.is_XHZY_d = motor_IN_Int2_i;
18697 motor_DWork.temporalCounter_i2 = 0U;
18698
18699 /* Entry 'Int2': '<S1>:344' */
18700 motor_DWork.chu_jd += 0.002;
18701
18702 /* Inport: '<Root>/JD_In' */
18703 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
18704 motor_B.JD_In_d = motor_U.JD_In;
18705 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
18706
18707 /* Inport: '<Root>/YJ_In' */
18708 motor_B.YJ_In = motor_U.YJ_In;
18709
18710 /* Inport: '<Root>/Encode_Sp' */
18711 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18712
18713 /* Inport: '<Root>/System_Order' */
18714 motor_B.Slect_port_h = motor_U.System_Order;
18715
18716 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18717 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18718 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18719 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18720 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18721 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18722 &motor_DWork.EN_extern, &motor_DWork.Forword,
18723 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18724 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18725 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18726 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18727 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18728 &motor_DWork.chu_jd);
18729
18730 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18731 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18732 if (tmp_1 < 65536.0) {
18733 if (tmp_1 >= 0.0) {
18734 tmp_0 = (uint16_T)tmp_1;
18735 } else {
18736 tmp_0 = 0U;
18737 }
18738 } else {
18739 tmp_0 = MAX_uint16_T;
18740 }
18741
18742 motor_Y.PWMOUT = tmp_0;
18743 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18744 if (tmp_1 < 2.147483648E+9) {
18745 if (tmp_1 >= -2.147483648E+9) {
18746 tmp = (int32_T)tmp_1;
18747 } else {
18748 tmp = MIN_int32_T;
18749 }
18750 } else {
18751 tmp = MAX_int32_T;
18752 }
18753
18754 motor_Y.JD_Out = tmp;
18755 tmp_1 = motor_B.Motor_XHZY.KP_e;
18756 if (tmp_1 < 2.147483648E+9) {
18757 if (tmp_1 >= -2.147483648E+9) {
18758 tmp = (int32_T)tmp_1;
18759 } else {
18760 tmp = MIN_int32_T;
18761 }
18762 } else {
18763 tmp = MAX_int32_T;
18764 }
18765
18766 motor_Y.JD_Error = tmp;
18767 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18768 } else {
18769 motor_DWork.chu_jd += 0.01;
18770
18771 /* Inport: '<Root>/JD_In' */
18772 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
18773 motor_B.JD_In_d = motor_U.JD_In;
18774 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
18775
18776 /* Inport: '<Root>/YJ_In' */
18777 motor_B.YJ_In = motor_U.YJ_In;
18778
18779 /* Inport: '<Root>/Encode_Sp' */
18780 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
18781
18782 /* Inport: '<Root>/System_Order' */
18783 motor_B.Slect_port_h = motor_U.System_Order;
18784
18785 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
18786 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
18787 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
18788 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
18789 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
18790 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
18791 &motor_DWork.EN_extern, &motor_DWork.Forword,
18792 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
18793 &motor_DWork.KG_En, &motor_DWork.KG_JD,
18794 &motor_DWork.KG_YJ, &motor_DWork.KG_clc,
18795 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
18796 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
18797 &motor_DWork.chu_jd);
18798
18799 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
18800 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
18801 if (tmp_1 < 65536.0) {
18802 if (tmp_1 >= 0.0) {
18803 tmp_0 = (uint16_T)tmp_1;
18804 } else {
18805 tmp_0 = 0U;
18806 }
18807 } else {
18808 tmp_0 = MAX_uint16_T;
18809 }
18810
18811 motor_Y.PWMOUT = tmp_0;
18812 tmp_1 = motor_B.Motor_XHZY.KP_JD;
18813 if (tmp_1 < 2.147483648E+9) {
18814 if (tmp_1 >= -2.147483648E+9) {
18815 tmp = (int32_T)tmp_1;
18816 } else {
18817 tmp = MIN_int32_T;
18818 }
18819 } else {
18820 tmp = MAX_int32_T;
18821 }
18822
18823 motor_Y.JD_Out = tmp;
18824 tmp_1 = motor_B.Motor_XHZY.KP_e;
18825 if (tmp_1 < 2.147483648E+9) {
18826 if (tmp_1 >= -2.147483648E+9) {
18827 tmp = (int32_T)tmp_1;
18828 } else {
18829 tmp = MIN_int32_T;
18830 }
18831 } else {
18832 tmp = MAX_int32_T;
18833 }
18834
18835 motor_Y.JD_Error = tmp;
18836 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
18837 }
18838 break;
18839 }
18840}
18841
18842/* Function for Chart: '<Root>/Chart' */
18843static void motor_Limit_Up_Test_a(void)
18844{
18845 int32_T tmp;
18846 real_T tmp_0;
18847 uint16_T tmp_1;
18848
18849 /* During 'Limit_Up_Test': '<S1>:340' */
18850 switch (motor_DWork.is_Limit_Up_Test_j) {
18851 case motor_IN_HY_f:
18852 /* During 'HY': '<S1>:349' */
18853 switch (motor_DWork.is_HY_l) {
18854 case motor_IN_Int:
18855 /* During 'Int': '<S1>:350' */
18856 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
18857 /* Transition: '<S1>:1012' */
18858 motor_DWork.is_HY_l = motor_IN_Int8;
18859 }
18860 break;
18861
18862 case motor_IN_Int1:
18863 /* Inport: '<Root>/Up_Limit' */
18864 /* During 'Int1': '<S1>:351' */
18865 if (motor_U.Up_Limit == 0) {
18866 /* Transition: '<S1>:794' */
18867 /* 上限位开关低电平有效 */
18868 motor_DWork.is_HY_l = motor_IN_Int2_i;
18869 motor_DWork.temporalCounter_i2 = 0U;
18870
18871 /* Entry 'Int2': '<S1>:352' */
18872 motor_DWork.chu_jd += 0.002;
18873
18874 /* Inport: '<Root>/JD_In' */
18875 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
18876 motor_B.JD_In_f = motor_U.JD_In;
18877 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
18878
18879 /* Inport: '<Root>/System_Order' */
18880 motor_B.Slect_port_c = motor_U.System_Order;
18881
18882 /* Inport: '<Root>/Encode_Sp' */
18883 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
18884
18885 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
18886 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
18887 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
18888 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
18889 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
18890 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
18891 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
18892 &motor_DWork.chu_jd);
18893
18894 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
18895 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
18896 tmp_0 = motor_B.Motor_HYDG.KP_e;
18897 if (tmp_0 < 2.147483648E+9) {
18898 if (tmp_0 >= -2.147483648E+9) {
18899 tmp = (int32_T)tmp_0;
18900 } else {
18901 tmp = MIN_int32_T;
18902 }
18903 } else {
18904 tmp = MAX_int32_T;
18905 }
18906
18907 motor_Y.JD_Error = tmp;
18908 tmp_0 = motor_B.Motor_HYDG.KP_JD;
18909 if (tmp_0 < 2.147483648E+9) {
18910 if (tmp_0 >= -2.147483648E+9) {
18911 tmp = (int32_T)tmp_0;
18912 } else {
18913 tmp = MIN_int32_T;
18914 }
18915 } else {
18916 tmp = MAX_int32_T;
18917 }
18918
18919 motor_Y.JD_Out = tmp;
18920 } else if (motor_DWork.temporalCounter_i2 >= (uint32_T)(7.0 /
18921 motor_DWork.Ts)) {
18922 /* Transition: '<S1>:1043' */
18923 motor_Y.Flag_Up_GZ_limit = 0U;
18924
18925 /* 上限位开关故障$/ */
18926 motor_DWork.is_HY_l = motor_IN_Int2_i;
18927 motor_DWork.temporalCounter_i2 = 0U;
18928
18929 /* Entry 'Int2': '<S1>:352' */
18930 motor_DWork.chu_jd += 0.002;
18931
18932 /* Inport: '<Root>/JD_In' */
18933 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
18934 motor_B.JD_In_f = motor_U.JD_In;
18935 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
18936
18937 /* Inport: '<Root>/System_Order' */
18938 motor_B.Slect_port_c = motor_U.System_Order;
18939
18940 /* Inport: '<Root>/Encode_Sp' */
18941 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
18942
18943 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
18944 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
18945 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
18946 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
18947 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
18948 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
18949 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
18950 &motor_DWork.chu_jd);
18951
18952 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
18953 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
18954 tmp_0 = motor_B.Motor_HYDG.KP_e;
18955 if (tmp_0 < 2.147483648E+9) {
18956 if (tmp_0 >= -2.147483648E+9) {
18957 tmp = (int32_T)tmp_0;
18958 } else {
18959 tmp = MIN_int32_T;
18960 }
18961 } else {
18962 tmp = MAX_int32_T;
18963 }
18964
18965 motor_Y.JD_Error = tmp;
18966 tmp_0 = motor_B.Motor_HYDG.KP_JD;
18967 if (tmp_0 < 2.147483648E+9) {
18968 if (tmp_0 >= -2.147483648E+9) {
18969 tmp = (int32_T)tmp_0;
18970 } else {
18971 tmp = MIN_int32_T;
18972 }
18973 } else {
18974 tmp = MAX_int32_T;
18975 }
18976
18977 motor_Y.JD_Out = tmp;
18978 } else {
18979 motor_DWork.chu_jd += 0.002;
18980
18981 /* Inport: '<Root>/JD_In' */
18982 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
18983 motor_B.JD_In_f = motor_U.JD_In;
18984 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
18985
18986 /* Inport: '<Root>/System_Order' */
18987 motor_B.Slect_port_c = motor_U.System_Order;
18988
18989 /* Inport: '<Root>/Encode_Sp' */
18990 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
18991
18992 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
18993 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
18994 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
18995 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
18996 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
18997 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
18998 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
18999 &motor_DWork.chu_jd);
19000
19001 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
19002 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
19003 tmp_0 = motor_B.Motor_HYDG.KP_e;
19004 if (tmp_0 < 2.147483648E+9) {
19005 if (tmp_0 >= -2.147483648E+9) {
19006 tmp = (int32_T)tmp_0;
19007 } else {
19008 tmp = MIN_int32_T;
19009 }
19010 } else {
19011 tmp = MAX_int32_T;
19012 }
19013
19014 motor_Y.JD_Error = tmp;
19015 tmp_0 = motor_B.Motor_HYDG.KP_JD;
19016 if (tmp_0 < 2.147483648E+9) {
19017 if (tmp_0 >= -2.147483648E+9) {
19018 tmp = (int32_T)tmp_0;
19019 } else {
19020 tmp = MIN_int32_T;
19021 }
19022 } else {
19023 tmp = MAX_int32_T;
19024 }
19025
19026 motor_Y.JD_Out = tmp;
19027 }
19028 break;
19029
19030 case motor_IN_Int2_i:
19031 /* During 'Int2': '<S1>:352' */
19032 if (motor_DWork.temporalCounter_i2 >= 50U) {
19033 /* Transition: '<S1>:796' */
19034 motor_DWork.is_HY_l = motor_IN_Int7;
19035 motor_DWork.temporalCounter_i2 = 0U;
19036 } else {
19037 motor_DWork.chu_jd += 0.002;
19038
19039 /* Inport: '<Root>/JD_In' */
19040 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
19041 motor_B.JD_In_f = motor_U.JD_In;
19042 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
19043
19044 /* Inport: '<Root>/System_Order' */
19045 motor_B.Slect_port_c = motor_U.System_Order;
19046
19047 /* Inport: '<Root>/Encode_Sp' */
19048 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
19049
19050 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
19051 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
19052 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
19053 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
19054 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
19055 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
19056 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19057 &motor_DWork.chu_jd);
19058
19059 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
19060 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
19061 tmp_0 = motor_B.Motor_HYDG.KP_e;
19062 if (tmp_0 < 2.147483648E+9) {
19063 if (tmp_0 >= -2.147483648E+9) {
19064 tmp = (int32_T)tmp_0;
19065 } else {
19066 tmp = MIN_int32_T;
19067 }
19068 } else {
19069 tmp = MAX_int32_T;
19070 }
19071
19072 motor_Y.JD_Error = tmp;
19073 tmp_0 = motor_B.Motor_HYDG.KP_JD;
19074 if (tmp_0 < 2.147483648E+9) {
19075 if (tmp_0 >= -2.147483648E+9) {
19076 tmp = (int32_T)tmp_0;
19077 } else {
19078 tmp = MIN_int32_T;
19079 }
19080 } else {
19081 tmp = MAX_int32_T;
19082 }
19083
19084 motor_Y.JD_Out = tmp;
19085 }
19086 break;
19087
19088 case motor_IN_Int3_l:
19089 /* During 'Int3': '<S1>:353' */
19090 if (fabs(motor_DWork.chu_jd) < 0.1) {
19091 /* Transition: '<S1>:797' */
19092 motor_DWork.is_HY_l = motor_IN_Int4_b;
19093 motor_DWork.temporalCounter_i2 = 0U;
19094
19095 /* Entry 'Int4': '<S1>:355' */
19096 motor_DWork.chu_jd = 0.0;
19097
19098 /* Inport: '<Root>/JD_In' */
19099 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
19100 motor_B.JD_In_f = motor_U.JD_In;
19101 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
19102
19103 /* Inport: '<Root>/System_Order' */
19104 motor_B.Slect_port_c = motor_U.System_Order;
19105
19106 /* Inport: '<Root>/Encode_Sp' */
19107 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
19108
19109 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
19110 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
19111 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
19112 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
19113 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
19114 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
19115 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19116 &motor_DWork.chu_jd);
19117
19118 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
19119 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
19120 tmp_0 = motor_B.Motor_HYDG.KP_e;
19121 if (tmp_0 < 2.147483648E+9) {
19122 if (tmp_0 >= -2.147483648E+9) {
19123 tmp = (int32_T)tmp_0;
19124 } else {
19125 tmp = MIN_int32_T;
19126 }
19127 } else {
19128 tmp = MAX_int32_T;
19129 }
19130
19131 motor_Y.JD_Error = tmp;
19132 tmp_0 = motor_B.Motor_HYDG.KP_JD;
19133 if (tmp_0 < 2.147483648E+9) {
19134 if (tmp_0 >= -2.147483648E+9) {
19135 tmp = (int32_T)tmp_0;
19136 } else {
19137 tmp = MIN_int32_T;
19138 }
19139 } else {
19140 tmp = MAX_int32_T;
19141 }
19142
19143 motor_Y.JD_Out = tmp;
19144 } else {
19145 motor_DWork.chu_jd -= 0.01;
19146
19147 /* Inport: '<Root>/JD_In' */
19148 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
19149 motor_B.JD_In_f = motor_U.JD_In;
19150 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
19151
19152 /* Inport: '<Root>/System_Order' */
19153 motor_B.Slect_port_c = motor_U.System_Order;
19154
19155 /* Inport: '<Root>/Encode_Sp' */
19156 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
19157
19158 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
19159 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
19160 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
19161 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
19162 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
19163 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
19164 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19165 &motor_DWork.chu_jd);
19166
19167 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
19168 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
19169 tmp_0 = motor_B.Motor_HYDG.KP_e;
19170 if (tmp_0 < 2.147483648E+9) {
19171 if (tmp_0 >= -2.147483648E+9) {
19172 tmp = (int32_T)tmp_0;
19173 } else {
19174 tmp = MIN_int32_T;
19175 }
19176 } else {
19177 tmp = MAX_int32_T;
19178 }
19179
19180 motor_Y.JD_Error = tmp;
19181 tmp_0 = motor_B.Motor_HYDG.KP_JD;
19182 if (tmp_0 < 2.147483648E+9) {
19183 if (tmp_0 >= -2.147483648E+9) {
19184 tmp = (int32_T)tmp_0;
19185 } else {
19186 tmp = MIN_int32_T;
19187 }
19188 } else {
19189 tmp = MAX_int32_T;
19190 }
19191
19192 motor_Y.JD_Out = tmp;
19193 }
19194 break;
19195
19196 case motor_IN_Int4_b:
19197 /* During 'Int4': '<S1>:355' */
19198 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(2.0 / motor_DWork.Ts)) {
19199 /* Transition: '<S1>:799' */
19200 motor_DWork.is_HY_l = motor_IN_Int5_a;
19201 motor_DWork.temporalCounter_i2 = 0U;
19202
19203 /* Entry 'Int5': '<S1>:356' */
19204 motor_Y.Motor_En = true;
19205
19206 /* 电机失能 */
19207 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
19208 } else {
19209 /* Inport: '<Root>/JD_In' */
19210 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
19211 motor_B.JD_In_f = motor_U.JD_In;
19212 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
19213
19214 /* Inport: '<Root>/System_Order' */
19215 motor_B.Slect_port_c = motor_U.System_Order;
19216
19217 /* Inport: '<Root>/Encode_Sp' */
19218 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
19219
19220 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
19221 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
19222 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
19223 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
19224 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
19225 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
19226 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19227 &motor_DWork.chu_jd);
19228
19229 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
19230 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
19231 tmp_0 = motor_B.Motor_HYDG.KP_e;
19232 if (tmp_0 < 2.147483648E+9) {
19233 if (tmp_0 >= -2.147483648E+9) {
19234 tmp = (int32_T)tmp_0;
19235 } else {
19236 tmp = MIN_int32_T;
19237 }
19238 } else {
19239 tmp = MAX_int32_T;
19240 }
19241
19242 motor_Y.JD_Error = tmp;
19243 tmp_0 = motor_B.Motor_HYDG.KP_JD;
19244 if (tmp_0 < 2.147483648E+9) {
19245 if (tmp_0 >= -2.147483648E+9) {
19246 tmp = (int32_T)tmp_0;
19247 } else {
19248 tmp = MIN_int32_T;
19249 }
19250 } else {
19251 tmp = MAX_int32_T;
19252 }
19253
19254 motor_Y.JD_Out = tmp;
19255 }
19256 break;
19257
19258 case motor_IN_Int5_a:
19259 /* During 'Int5': '<S1>:356' */
19260 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
19261 /* Transition: '<S1>:798' */
19262 motor_DWork.is_HY_l = motor_IN_Int6_m;
19263
19264 /* Entry 'Int6': '<S1>:354' */
19265 motor_Y.DCZD = false;
19266
19267 /* 解除制动 */
19268 }
19269 break;
19270
19271 case motor_IN_Int6_m:
19272 /* During 'Int6': '<S1>:354' */
19273 /* Transition: '<S1>:795' */
19274 /* Transition: '<S1>:773' */
19275 /* Transition: '<S1>:620' */
19276 motor_DWork.XiaoDaShen = 0.0;
19277 motor_DWork.is_HY_l = motor_IN_NO_ACTIVE_CHILD;
19278 motor_DWork.is_Limit_Up_Test_j = motor_IN_NO_ACTIVE_CHILD;
19279 motor_DWork.is_Algorithm_e = motor_IN_Defualt;
19280
19281 /* Entry 'Defualt': '<S1>:206' */
19282 motor_DWork.chu_jd = 0.0;
19283 motor_DWork.KG = 0U;
19284 motor_Y.Motor_En = false;
19285 motor_Y.DCZD = false;
19286
19287 /* 开电磁制动,低有效 */
19288 break;
19289
19290 case motor_IN_Int7:
19291 /* During 'Int7': '<S1>:957' */
19292 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
19293 /* Transition: '<S1>:958' */
19294 motor_DWork.is_HY_l = motor_IN_Int3_l;
19295
19296 /* Entry 'Int3': '<S1>:353' */
19297 motor_DWork.chu_jd -= 0.01;
19298
19299 /* Inport: '<Root>/JD_In' */
19300 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
19301 motor_B.JD_In_f = motor_U.JD_In;
19302 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
19303
19304 /* Inport: '<Root>/System_Order' */
19305 motor_B.Slect_port_c = motor_U.System_Order;
19306
19307 /* Inport: '<Root>/Encode_Sp' */
19308 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
19309
19310 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
19311 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
19312 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
19313 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
19314 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
19315 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
19316 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19317 &motor_DWork.chu_jd);
19318
19319 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
19320 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
19321 tmp_0 = motor_B.Motor_HYDG.KP_e;
19322 if (tmp_0 < 2.147483648E+9) {
19323 if (tmp_0 >= -2.147483648E+9) {
19324 tmp = (int32_T)tmp_0;
19325 } else {
19326 tmp = MIN_int32_T;
19327 }
19328 } else {
19329 tmp = MAX_int32_T;
19330 }
19331
19332 motor_Y.JD_Error = tmp;
19333 tmp_0 = motor_B.Motor_HYDG.KP_JD;
19334 if (tmp_0 < 2.147483648E+9) {
19335 if (tmp_0 >= -2.147483648E+9) {
19336 tmp = (int32_T)tmp_0;
19337 } else {
19338 tmp = MIN_int32_T;
19339 }
19340 } else {
19341 tmp = MAX_int32_T;
19342 }
19343
19344 motor_Y.JD_Out = tmp;
19345 } else {
19346 /* Inport: '<Root>/JD_In' */
19347 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
19348 motor_B.JD_In_f = motor_U.JD_In;
19349 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
19350
19351 /* Inport: '<Root>/System_Order' */
19352 motor_B.Slect_port_c = motor_U.System_Order;
19353
19354 /* Inport: '<Root>/Encode_Sp' */
19355 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
19356
19357 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
19358 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
19359 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
19360 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
19361 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
19362 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
19363 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19364 &motor_DWork.chu_jd);
19365
19366 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
19367 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
19368 tmp_0 = motor_B.Motor_HYDG.KP_e;
19369 if (tmp_0 < 2.147483648E+9) {
19370 if (tmp_0 >= -2.147483648E+9) {
19371 tmp = (int32_T)tmp_0;
19372 } else {
19373 tmp = MIN_int32_T;
19374 }
19375 } else {
19376 tmp = MAX_int32_T;
19377 }
19378
19379 motor_Y.JD_Error = tmp;
19380 tmp_0 = motor_B.Motor_HYDG.KP_JD;
19381 if (tmp_0 < 2.147483648E+9) {
19382 if (tmp_0 >= -2.147483648E+9) {
19383 tmp = (int32_T)tmp_0;
19384 } else {
19385 tmp = MIN_int32_T;
19386 }
19387 } else {
19388 tmp = MAX_int32_T;
19389 }
19390
19391 motor_Y.JD_Out = tmp;
19392 }
19393 break;
19394
19395 default:
19396 /* During 'Int8': '<S1>:1011' */
19397 if (motor_DWork.chu_jd > 14.0) {
19398 /* Transition: '<S1>:1010' */
19399 motor_DWork.is_HY_l = motor_IN_Int1;
19400 motor_DWork.temporalCounter_i2 = 0U;
19401
19402 /* Entry 'Int1': '<S1>:351' */
19403 motor_DWork.chu_jd += 0.002;
19404
19405 /* Inport: '<Root>/JD_In' */
19406 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
19407 motor_B.JD_In_f = motor_U.JD_In;
19408 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
19409
19410 /* Inport: '<Root>/System_Order' */
19411 motor_B.Slect_port_c = motor_U.System_Order;
19412
19413 /* Inport: '<Root>/Encode_Sp' */
19414 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
19415
19416 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
19417 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
19418 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
19419 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
19420 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
19421 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
19422 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19423 &motor_DWork.chu_jd);
19424
19425 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
19426 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
19427 tmp_0 = motor_B.Motor_HYDG.KP_e;
19428 if (tmp_0 < 2.147483648E+9) {
19429 if (tmp_0 >= -2.147483648E+9) {
19430 tmp = (int32_T)tmp_0;
19431 } else {
19432 tmp = MIN_int32_T;
19433 }
19434 } else {
19435 tmp = MAX_int32_T;
19436 }
19437
19438 motor_Y.JD_Error = tmp;
19439 tmp_0 = motor_B.Motor_HYDG.KP_JD;
19440 if (tmp_0 < 2.147483648E+9) {
19441 if (tmp_0 >= -2.147483648E+9) {
19442 tmp = (int32_T)tmp_0;
19443 } else {
19444 tmp = MIN_int32_T;
19445 }
19446 } else {
19447 tmp = MAX_int32_T;
19448 }
19449
19450 motor_Y.JD_Out = tmp;
19451 } else if (motor_U.Up_Limit == 0) {
19452 /* Transition: '<S1>:1009' */
19453 /* 上限位开关低电平有效 */
19454 motor_DWork.is_HY_l = motor_IN_Int2_i;
19455 motor_DWork.temporalCounter_i2 = 0U;
19456
19457 /* Entry 'Int2': '<S1>:352' */
19458 motor_DWork.chu_jd += 0.002;
19459
19460 /* Inport: '<Root>/JD_In' */
19461 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
19462 motor_B.JD_In_f = motor_U.JD_In;
19463 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
19464
19465 /* Inport: '<Root>/System_Order' */
19466 motor_B.Slect_port_c = motor_U.System_Order;
19467
19468 /* Inport: '<Root>/Encode_Sp' */
19469 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
19470
19471 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
19472 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
19473 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
19474 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
19475 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
19476 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
19477 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19478 &motor_DWork.chu_jd);
19479
19480 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
19481 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
19482 tmp_0 = motor_B.Motor_HYDG.KP_e;
19483 if (tmp_0 < 2.147483648E+9) {
19484 if (tmp_0 >= -2.147483648E+9) {
19485 tmp = (int32_T)tmp_0;
19486 } else {
19487 tmp = MIN_int32_T;
19488 }
19489 } else {
19490 tmp = MAX_int32_T;
19491 }
19492
19493 motor_Y.JD_Error = tmp;
19494 tmp_0 = motor_B.Motor_HYDG.KP_JD;
19495 if (tmp_0 < 2.147483648E+9) {
19496 if (tmp_0 >= -2.147483648E+9) {
19497 tmp = (int32_T)tmp_0;
19498 } else {
19499 tmp = MIN_int32_T;
19500 }
19501 } else {
19502 tmp = MAX_int32_T;
19503 }
19504
19505 motor_Y.JD_Out = tmp;
19506 } else {
19507 motor_DWork.chu_jd += 0.01;
19508
19509 /* Inport: '<Root>/JD_In' */
19510 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
19511 motor_B.JD_In_f = motor_U.JD_In;
19512 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
19513
19514 /* Inport: '<Root>/System_Order' */
19515 motor_B.Slect_port_c = motor_U.System_Order;
19516
19517 /* Inport: '<Root>/Encode_Sp' */
19518 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
19519
19520 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
19521 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
19522 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
19523 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
19524 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
19525 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
19526 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19527 &motor_DWork.chu_jd);
19528
19529 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
19530 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
19531 tmp_0 = motor_B.Motor_HYDG.KP_e;
19532 if (tmp_0 < 2.147483648E+9) {
19533 if (tmp_0 >= -2.147483648E+9) {
19534 tmp = (int32_T)tmp_0;
19535 } else {
19536 tmp = MIN_int32_T;
19537 }
19538 } else {
19539 tmp = MAX_int32_T;
19540 }
19541
19542 motor_Y.JD_Error = tmp;
19543 tmp_0 = motor_B.Motor_HYDG.KP_JD;
19544 if (tmp_0 < 2.147483648E+9) {
19545 if (tmp_0 >= -2.147483648E+9) {
19546 tmp = (int32_T)tmp_0;
19547 } else {
19548 tmp = MIN_int32_T;
19549 }
19550 } else {
19551 tmp = MAX_int32_T;
19552 }
19553
19554 motor_Y.JD_Out = tmp;
19555 }
19556 break;
19557 }
19558 break;
19559
19560 case motor_IN_XHHY_m1:
19561 /* During 'XHHY': '<S1>:357' */
19562 switch (motor_DWork.is_XHHY_nb) {
19563 case motor_IN_Int:
19564 /* During 'Int': '<S1>:358' */
19565 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
19566 /* Transition: '<S1>:1014' */
19567 motor_DWork.is_XHHY_nb = motor_IN_Int8;
19568 }
19569 break;
19570
19571 case motor_IN_Int1:
19572 /* Inport: '<Root>/Up_Limit' */
19573 /* During 'Int1': '<S1>:359' */
19574 if (motor_U.Up_Limit == 0) {
19575 /* Transition: '<S1>:802' */
19576 /* 上限位开关低电平有效 */
19577 motor_DWork.is_XHHY_nb = motor_IN_Int2_i;
19578 motor_DWork.temporalCounter_i2 = 0U;
19579
19580 /* Entry 'Int2': '<S1>:360' */
19581 motor_DWork.chu_jd += 0.002;
19582
19583 /* Inport: '<Root>/JD_In' */
19584 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
19585 motor_B.JD_In = motor_U.JD_In;
19586 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
19587
19588 /* Inport: '<Root>/Encode_Sp' */
19589 motor_B.Encode_Sp = motor_U.Encode_Sp;
19590
19591 /* Inport: '<Root>/System_Order' */
19592 motor_B.Slect_port = motor_U.System_Order;
19593
19594 /* Inport: '<Root>/SGWY_In' */
19595 motor_B.SGWY_In = motor_U.SGWY_In;
19596
19597 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
19598 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
19599 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
19600 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
19601 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
19602 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
19603 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
19604 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
19605 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19606 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
19607 &motor_DWork.chu_jd);
19608
19609 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
19610 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
19611 if (tmp_0 < 65536.0) {
19612 if (tmp_0 >= 0.0) {
19613 tmp_1 = (uint16_T)tmp_0;
19614 } else {
19615 tmp_1 = 0U;
19616 }
19617 } else {
19618 tmp_1 = MAX_uint16_T;
19619 }
19620
19621 motor_Y.PWMOUT = tmp_1;
19622 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
19623 if (tmp_0 < 2.147483648E+9) {
19624 if (tmp_0 >= -2.147483648E+9) {
19625 tmp = (int32_T)tmp_0;
19626 } else {
19627 tmp = MIN_int32_T;
19628 }
19629 } else {
19630 tmp = MAX_int32_T;
19631 }
19632
19633 motor_Y.JD_Out = tmp;
19634 tmp_0 = motor_B.Motor_XHHY.KP_e;
19635 if (tmp_0 < 2.147483648E+9) {
19636 if (tmp_0 >= -2.147483648E+9) {
19637 tmp = (int32_T)tmp_0;
19638 } else {
19639 tmp = MIN_int32_T;
19640 }
19641 } else {
19642 tmp = MAX_int32_T;
19643 }
19644
19645 motor_Y.JD_Error = tmp;
19646 } else if (motor_DWork.temporalCounter_i2 >= (uint32_T)(5.0 /
19647 motor_DWork.Ts)) {
19648 /* Transition: '<S1>:1044' */
19649 motor_Y.Flag_Up_GZ_limit = 0U;
19650
19651 /* 上限位开关故障$/ */
19652 motor_DWork.is_XHHY_nb = motor_IN_Int2_i;
19653 motor_DWork.temporalCounter_i2 = 0U;
19654
19655 /* Entry 'Int2': '<S1>:360' */
19656 motor_DWork.chu_jd += 0.002;
19657
19658 /* Inport: '<Root>/JD_In' */
19659 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
19660 motor_B.JD_In = motor_U.JD_In;
19661 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
19662
19663 /* Inport: '<Root>/Encode_Sp' */
19664 motor_B.Encode_Sp = motor_U.Encode_Sp;
19665
19666 /* Inport: '<Root>/System_Order' */
19667 motor_B.Slect_port = motor_U.System_Order;
19668
19669 /* Inport: '<Root>/SGWY_In' */
19670 motor_B.SGWY_In = motor_U.SGWY_In;
19671
19672 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
19673 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
19674 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
19675 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
19676 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
19677 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
19678 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
19679 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
19680 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19681 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
19682 &motor_DWork.chu_jd);
19683
19684 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
19685 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
19686 if (tmp_0 < 65536.0) {
19687 if (tmp_0 >= 0.0) {
19688 tmp_1 = (uint16_T)tmp_0;
19689 } else {
19690 tmp_1 = 0U;
19691 }
19692 } else {
19693 tmp_1 = MAX_uint16_T;
19694 }
19695
19696 motor_Y.PWMOUT = tmp_1;
19697 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
19698 if (tmp_0 < 2.147483648E+9) {
19699 if (tmp_0 >= -2.147483648E+9) {
19700 tmp = (int32_T)tmp_0;
19701 } else {
19702 tmp = MIN_int32_T;
19703 }
19704 } else {
19705 tmp = MAX_int32_T;
19706 }
19707
19708 motor_Y.JD_Out = tmp;
19709 tmp_0 = motor_B.Motor_XHHY.KP_e;
19710 if (tmp_0 < 2.147483648E+9) {
19711 if (tmp_0 >= -2.147483648E+9) {
19712 tmp = (int32_T)tmp_0;
19713 } else {
19714 tmp = MIN_int32_T;
19715 }
19716 } else {
19717 tmp = MAX_int32_T;
19718 }
19719
19720 motor_Y.JD_Error = tmp;
19721 } else {
19722 motor_DWork.chu_jd += 0.002;
19723
19724 /* Inport: '<Root>/JD_In' */
19725 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
19726 motor_B.JD_In = motor_U.JD_In;
19727 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
19728
19729 /* Inport: '<Root>/Encode_Sp' */
19730 motor_B.Encode_Sp = motor_U.Encode_Sp;
19731
19732 /* Inport: '<Root>/System_Order' */
19733 motor_B.Slect_port = motor_U.System_Order;
19734
19735 /* Inport: '<Root>/SGWY_In' */
19736 motor_B.SGWY_In = motor_U.SGWY_In;
19737
19738 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
19739 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
19740 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
19741 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
19742 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
19743 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
19744 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
19745 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
19746 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19747 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
19748 &motor_DWork.chu_jd);
19749
19750 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
19751 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
19752 if (tmp_0 < 65536.0) {
19753 if (tmp_0 >= 0.0) {
19754 tmp_1 = (uint16_T)tmp_0;
19755 } else {
19756 tmp_1 = 0U;
19757 }
19758 } else {
19759 tmp_1 = MAX_uint16_T;
19760 }
19761
19762 motor_Y.PWMOUT = tmp_1;
19763 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
19764 if (tmp_0 < 2.147483648E+9) {
19765 if (tmp_0 >= -2.147483648E+9) {
19766 tmp = (int32_T)tmp_0;
19767 } else {
19768 tmp = MIN_int32_T;
19769 }
19770 } else {
19771 tmp = MAX_int32_T;
19772 }
19773
19774 motor_Y.JD_Out = tmp;
19775 tmp_0 = motor_B.Motor_XHHY.KP_e;
19776 if (tmp_0 < 2.147483648E+9) {
19777 if (tmp_0 >= -2.147483648E+9) {
19778 tmp = (int32_T)tmp_0;
19779 } else {
19780 tmp = MIN_int32_T;
19781 }
19782 } else {
19783 tmp = MAX_int32_T;
19784 }
19785
19786 motor_Y.JD_Error = tmp;
19787 }
19788 break;
19789
19790 case motor_IN_Int2_i:
19791 /* During 'Int2': '<S1>:360' */
19792 if (motor_DWork.temporalCounter_i2 >= 50U) {
19793 /* Transition: '<S1>:804' */
19794 motor_DWork.is_XHHY_nb = motor_IN_Int7;
19795 motor_DWork.temporalCounter_i2 = 0U;
19796 } else {
19797 motor_DWork.chu_jd += 0.002;
19798
19799 /* Inport: '<Root>/JD_In' */
19800 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
19801 motor_B.JD_In = motor_U.JD_In;
19802 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
19803
19804 /* Inport: '<Root>/Encode_Sp' */
19805 motor_B.Encode_Sp = motor_U.Encode_Sp;
19806
19807 /* Inport: '<Root>/System_Order' */
19808 motor_B.Slect_port = motor_U.System_Order;
19809
19810 /* Inport: '<Root>/SGWY_In' */
19811 motor_B.SGWY_In = motor_U.SGWY_In;
19812
19813 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
19814 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
19815 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
19816 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
19817 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
19818 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
19819 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
19820 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
19821 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19822 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
19823 &motor_DWork.chu_jd);
19824
19825 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
19826 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
19827 if (tmp_0 < 65536.0) {
19828 if (tmp_0 >= 0.0) {
19829 tmp_1 = (uint16_T)tmp_0;
19830 } else {
19831 tmp_1 = 0U;
19832 }
19833 } else {
19834 tmp_1 = MAX_uint16_T;
19835 }
19836
19837 motor_Y.PWMOUT = tmp_1;
19838 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
19839 if (tmp_0 < 2.147483648E+9) {
19840 if (tmp_0 >= -2.147483648E+9) {
19841 tmp = (int32_T)tmp_0;
19842 } else {
19843 tmp = MIN_int32_T;
19844 }
19845 } else {
19846 tmp = MAX_int32_T;
19847 }
19848
19849 motor_Y.JD_Out = tmp;
19850 tmp_0 = motor_B.Motor_XHHY.KP_e;
19851 if (tmp_0 < 2.147483648E+9) {
19852 if (tmp_0 >= -2.147483648E+9) {
19853 tmp = (int32_T)tmp_0;
19854 } else {
19855 tmp = MIN_int32_T;
19856 }
19857 } else {
19858 tmp = MAX_int32_T;
19859 }
19860
19861 motor_Y.JD_Error = tmp;
19862 }
19863 break;
19864
19865 case motor_IN_Int3_l:
19866 /* During 'Int3': '<S1>:361' */
19867 if (fabs(motor_DWork.chu_jd) < 0.1) {
19868 /* Transition: '<S1>:805' */
19869 motor_DWork.is_XHHY_nb = motor_IN_Int4_b;
19870 motor_DWork.temporalCounter_i2 = 0U;
19871
19872 /* Entry 'Int4': '<S1>:363' */
19873 motor_DWork.chu_jd = 0.0;
19874
19875 /* Inport: '<Root>/JD_In' */
19876 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
19877 motor_B.JD_In = motor_U.JD_In;
19878 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
19879
19880 /* Inport: '<Root>/Encode_Sp' */
19881 motor_B.Encode_Sp = motor_U.Encode_Sp;
19882
19883 /* Inport: '<Root>/System_Order' */
19884 motor_B.Slect_port = motor_U.System_Order;
19885
19886 /* Inport: '<Root>/SGWY_In' */
19887 motor_B.SGWY_In = motor_U.SGWY_In;
19888
19889 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
19890 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
19891 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
19892 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
19893 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
19894 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
19895 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
19896 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
19897 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19898 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
19899 &motor_DWork.chu_jd);
19900
19901 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
19902 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
19903 if (tmp_0 < 65536.0) {
19904 if (tmp_0 >= 0.0) {
19905 tmp_1 = (uint16_T)tmp_0;
19906 } else {
19907 tmp_1 = 0U;
19908 }
19909 } else {
19910 tmp_1 = MAX_uint16_T;
19911 }
19912
19913 motor_Y.PWMOUT = tmp_1;
19914 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
19915 if (tmp_0 < 2.147483648E+9) {
19916 if (tmp_0 >= -2.147483648E+9) {
19917 tmp = (int32_T)tmp_0;
19918 } else {
19919 tmp = MIN_int32_T;
19920 }
19921 } else {
19922 tmp = MAX_int32_T;
19923 }
19924
19925 motor_Y.JD_Out = tmp;
19926 tmp_0 = motor_B.Motor_XHHY.KP_e;
19927 if (tmp_0 < 2.147483648E+9) {
19928 if (tmp_0 >= -2.147483648E+9) {
19929 tmp = (int32_T)tmp_0;
19930 } else {
19931 tmp = MIN_int32_T;
19932 }
19933 } else {
19934 tmp = MAX_int32_T;
19935 }
19936
19937 motor_Y.JD_Error = tmp;
19938 } else {
19939 motor_DWork.chu_jd -= 0.01;
19940
19941 /* Inport: '<Root>/JD_In' */
19942 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
19943 motor_B.JD_In = motor_U.JD_In;
19944 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
19945
19946 /* Inport: '<Root>/Encode_Sp' */
19947 motor_B.Encode_Sp = motor_U.Encode_Sp;
19948
19949 /* Inport: '<Root>/System_Order' */
19950 motor_B.Slect_port = motor_U.System_Order;
19951
19952 /* Inport: '<Root>/SGWY_In' */
19953 motor_B.SGWY_In = motor_U.SGWY_In;
19954
19955 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
19956 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
19957 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
19958 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
19959 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
19960 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
19961 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
19962 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
19963 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
19964 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
19965 &motor_DWork.chu_jd);
19966
19967 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
19968 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
19969 if (tmp_0 < 65536.0) {
19970 if (tmp_0 >= 0.0) {
19971 tmp_1 = (uint16_T)tmp_0;
19972 } else {
19973 tmp_1 = 0U;
19974 }
19975 } else {
19976 tmp_1 = MAX_uint16_T;
19977 }
19978
19979 motor_Y.PWMOUT = tmp_1;
19980 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
19981 if (tmp_0 < 2.147483648E+9) {
19982 if (tmp_0 >= -2.147483648E+9) {
19983 tmp = (int32_T)tmp_0;
19984 } else {
19985 tmp = MIN_int32_T;
19986 }
19987 } else {
19988 tmp = MAX_int32_T;
19989 }
19990
19991 motor_Y.JD_Out = tmp;
19992 tmp_0 = motor_B.Motor_XHHY.KP_e;
19993 if (tmp_0 < 2.147483648E+9) {
19994 if (tmp_0 >= -2.147483648E+9) {
19995 tmp = (int32_T)tmp_0;
19996 } else {
19997 tmp = MIN_int32_T;
19998 }
19999 } else {
20000 tmp = MAX_int32_T;
20001 }
20002
20003 motor_Y.JD_Error = tmp;
20004 }
20005 break;
20006
20007 case motor_IN_Int4_b:
20008 /* During 'Int4': '<S1>:363' */
20009 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(2.0 / motor_DWork.Ts)) {
20010 /* Transition: '<S1>:560' */
20011 motor_DWork.is_XHHY_nb = motor_IN_Int5_a;
20012 motor_DWork.temporalCounter_i2 = 0U;
20013
20014 /* Entry 'Int5': '<S1>:143' */
20015 motor_Y.Motor_En = true;
20016
20017 /* 电机失能 */
20018 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
20019 } else {
20020 /* Inport: '<Root>/JD_In' */
20021 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
20022 motor_B.JD_In = motor_U.JD_In;
20023 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
20024
20025 /* Inport: '<Root>/Encode_Sp' */
20026 motor_B.Encode_Sp = motor_U.Encode_Sp;
20027
20028 /* Inport: '<Root>/System_Order' */
20029 motor_B.Slect_port = motor_U.System_Order;
20030
20031 /* Inport: '<Root>/SGWY_In' */
20032 motor_B.SGWY_In = motor_U.SGWY_In;
20033
20034 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
20035 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
20036 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
20037 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
20038 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
20039 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
20040 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
20041 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
20042 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
20043 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
20044 &motor_DWork.chu_jd);
20045
20046 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
20047 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
20048 if (tmp_0 < 65536.0) {
20049 if (tmp_0 >= 0.0) {
20050 tmp_1 = (uint16_T)tmp_0;
20051 } else {
20052 tmp_1 = 0U;
20053 }
20054 } else {
20055 tmp_1 = MAX_uint16_T;
20056 }
20057
20058 motor_Y.PWMOUT = tmp_1;
20059 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
20060 if (tmp_0 < 2.147483648E+9) {
20061 if (tmp_0 >= -2.147483648E+9) {
20062 tmp = (int32_T)tmp_0;
20063 } else {
20064 tmp = MIN_int32_T;
20065 }
20066 } else {
20067 tmp = MAX_int32_T;
20068 }
20069
20070 motor_Y.JD_Out = tmp;
20071 tmp_0 = motor_B.Motor_XHHY.KP_e;
20072 if (tmp_0 < 2.147483648E+9) {
20073 if (tmp_0 >= -2.147483648E+9) {
20074 tmp = (int32_T)tmp_0;
20075 } else {
20076 tmp = MIN_int32_T;
20077 }
20078 } else {
20079 tmp = MAX_int32_T;
20080 }
20081
20082 motor_Y.JD_Error = tmp;
20083 }
20084 break;
20085
20086 case motor_IN_Int5_a:
20087 /* During 'Int5': '<S1>:143' */
20088 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
20089 /* Transition: '<S1>:534' */
20090 motor_DWork.is_XHHY_nb = motor_IN_Int6_m;
20091
20092 /* Entry 'Int6': '<S1>:362' */
20093 motor_Y.DCZD = true;
20094
20095 /* 开启制动 */
20096 }
20097 break;
20098
20099 case motor_IN_Int6_m:
20100 /* During 'Int6': '<S1>:362' */
20101 /* Transition: '<S1>:803' */
20102 /* Transition: '<S1>:773' */
20103 /* Transition: '<S1>:620' */
20104 motor_DWork.XiaoDaShen = 0.0;
20105 motor_DWork.is_XHHY_nb = motor_IN_NO_ACTIVE_CHILD;
20106 motor_DWork.is_Limit_Up_Test_j = motor_IN_NO_ACTIVE_CHILD;
20107 motor_DWork.is_Algorithm_e = motor_IN_Defualt;
20108
20109 /* Entry 'Defualt': '<S1>:206' */
20110 motor_DWork.chu_jd = 0.0;
20111 motor_DWork.KG = 0U;
20112 motor_Y.Motor_En = false;
20113 motor_Y.DCZD = false;
20114
20115 /* 开电磁制动,低有效 */
20116 break;
20117
20118 case motor_IN_Int7:
20119 /* During 'Int7': '<S1>:959' */
20120 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
20121 /* Transition: '<S1>:960' */
20122 motor_DWork.is_XHHY_nb = motor_IN_Int3_l;
20123
20124 /* Entry 'Int3': '<S1>:361' */
20125 motor_DWork.chu_jd -= 0.01;
20126
20127 /* Inport: '<Root>/JD_In' */
20128 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
20129 motor_B.JD_In = motor_U.JD_In;
20130 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
20131
20132 /* Inport: '<Root>/Encode_Sp' */
20133 motor_B.Encode_Sp = motor_U.Encode_Sp;
20134
20135 /* Inport: '<Root>/System_Order' */
20136 motor_B.Slect_port = motor_U.System_Order;
20137
20138 /* Inport: '<Root>/SGWY_In' */
20139 motor_B.SGWY_In = motor_U.SGWY_In;
20140
20141 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
20142 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
20143 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
20144 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
20145 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
20146 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
20147 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
20148 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
20149 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
20150 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
20151 &motor_DWork.chu_jd);
20152
20153 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
20154 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
20155 if (tmp_0 < 65536.0) {
20156 if (tmp_0 >= 0.0) {
20157 tmp_1 = (uint16_T)tmp_0;
20158 } else {
20159 tmp_1 = 0U;
20160 }
20161 } else {
20162 tmp_1 = MAX_uint16_T;
20163 }
20164
20165 motor_Y.PWMOUT = tmp_1;
20166 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
20167 if (tmp_0 < 2.147483648E+9) {
20168 if (tmp_0 >= -2.147483648E+9) {
20169 tmp = (int32_T)tmp_0;
20170 } else {
20171 tmp = MIN_int32_T;
20172 }
20173 } else {
20174 tmp = MAX_int32_T;
20175 }
20176
20177 motor_Y.JD_Out = tmp;
20178 tmp_0 = motor_B.Motor_XHHY.KP_e;
20179 if (tmp_0 < 2.147483648E+9) {
20180 if (tmp_0 >= -2.147483648E+9) {
20181 tmp = (int32_T)tmp_0;
20182 } else {
20183 tmp = MIN_int32_T;
20184 }
20185 } else {
20186 tmp = MAX_int32_T;
20187 }
20188
20189 motor_Y.JD_Error = tmp;
20190 } else {
20191 /* Inport: '<Root>/JD_In' */
20192 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
20193 motor_B.JD_In = motor_U.JD_In;
20194 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
20195
20196 /* Inport: '<Root>/Encode_Sp' */
20197 motor_B.Encode_Sp = motor_U.Encode_Sp;
20198
20199 /* Inport: '<Root>/System_Order' */
20200 motor_B.Slect_port = motor_U.System_Order;
20201
20202 /* Inport: '<Root>/SGWY_In' */
20203 motor_B.SGWY_In = motor_U.SGWY_In;
20204
20205 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
20206 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
20207 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
20208 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
20209 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
20210 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
20211 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
20212 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
20213 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
20214 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
20215 &motor_DWork.chu_jd);
20216
20217 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
20218 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
20219 if (tmp_0 < 65536.0) {
20220 if (tmp_0 >= 0.0) {
20221 tmp_1 = (uint16_T)tmp_0;
20222 } else {
20223 tmp_1 = 0U;
20224 }
20225 } else {
20226 tmp_1 = MAX_uint16_T;
20227 }
20228
20229 motor_Y.PWMOUT = tmp_1;
20230 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
20231 if (tmp_0 < 2.147483648E+9) {
20232 if (tmp_0 >= -2.147483648E+9) {
20233 tmp = (int32_T)tmp_0;
20234 } else {
20235 tmp = MIN_int32_T;
20236 }
20237 } else {
20238 tmp = MAX_int32_T;
20239 }
20240
20241 motor_Y.JD_Out = tmp;
20242 tmp_0 = motor_B.Motor_XHHY.KP_e;
20243 if (tmp_0 < 2.147483648E+9) {
20244 if (tmp_0 >= -2.147483648E+9) {
20245 tmp = (int32_T)tmp_0;
20246 } else {
20247 tmp = MIN_int32_T;
20248 }
20249 } else {
20250 tmp = MAX_int32_T;
20251 }
20252
20253 motor_Y.JD_Error = tmp;
20254 }
20255 break;
20256
20257 default:
20258 /* During 'Int8': '<S1>:1016' */
20259 if (motor_DWork.chu_jd > 16.0) {
20260 /* Transition: '<S1>:1015' */
20261 motor_DWork.is_XHHY_nb = motor_IN_Int1;
20262 motor_DWork.temporalCounter_i2 = 0U;
20263
20264 /* Entry 'Int1': '<S1>:359' */
20265 motor_DWork.chu_jd += 0.002;
20266
20267 /* Inport: '<Root>/JD_In' */
20268 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
20269 motor_B.JD_In = motor_U.JD_In;
20270 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
20271
20272 /* Inport: '<Root>/Encode_Sp' */
20273 motor_B.Encode_Sp = motor_U.Encode_Sp;
20274
20275 /* Inport: '<Root>/System_Order' */
20276 motor_B.Slect_port = motor_U.System_Order;
20277
20278 /* Inport: '<Root>/SGWY_In' */
20279 motor_B.SGWY_In = motor_U.SGWY_In;
20280
20281 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
20282 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
20283 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
20284 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
20285 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
20286 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
20287 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
20288 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
20289 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
20290 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
20291 &motor_DWork.chu_jd);
20292
20293 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
20294 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
20295 if (tmp_0 < 65536.0) {
20296 if (tmp_0 >= 0.0) {
20297 tmp_1 = (uint16_T)tmp_0;
20298 } else {
20299 tmp_1 = 0U;
20300 }
20301 } else {
20302 tmp_1 = MAX_uint16_T;
20303 }
20304
20305 motor_Y.PWMOUT = tmp_1;
20306 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
20307 if (tmp_0 < 2.147483648E+9) {
20308 if (tmp_0 >= -2.147483648E+9) {
20309 tmp = (int32_T)tmp_0;
20310 } else {
20311 tmp = MIN_int32_T;
20312 }
20313 } else {
20314 tmp = MAX_int32_T;
20315 }
20316
20317 motor_Y.JD_Out = tmp;
20318 tmp_0 = motor_B.Motor_XHHY.KP_e;
20319 if (tmp_0 < 2.147483648E+9) {
20320 if (tmp_0 >= -2.147483648E+9) {
20321 tmp = (int32_T)tmp_0;
20322 } else {
20323 tmp = MIN_int32_T;
20324 }
20325 } else {
20326 tmp = MAX_int32_T;
20327 }
20328
20329 motor_Y.JD_Error = tmp;
20330 } else if (motor_U.Up_Limit == 0) {
20331 /* Transition: '<S1>:1013' */
20332 /* 上限位开关低电平有效 */
20333 motor_DWork.is_XHHY_nb = motor_IN_Int2_i;
20334 motor_DWork.temporalCounter_i2 = 0U;
20335
20336 /* Entry 'Int2': '<S1>:360' */
20337 motor_DWork.chu_jd += 0.002;
20338
20339 /* Inport: '<Root>/JD_In' */
20340 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
20341 motor_B.JD_In = motor_U.JD_In;
20342 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
20343
20344 /* Inport: '<Root>/Encode_Sp' */
20345 motor_B.Encode_Sp = motor_U.Encode_Sp;
20346
20347 /* Inport: '<Root>/System_Order' */
20348 motor_B.Slect_port = motor_U.System_Order;
20349
20350 /* Inport: '<Root>/SGWY_In' */
20351 motor_B.SGWY_In = motor_U.SGWY_In;
20352
20353 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
20354 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
20355 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
20356 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
20357 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
20358 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
20359 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
20360 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
20361 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
20362 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
20363 &motor_DWork.chu_jd);
20364
20365 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
20366 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
20367 if (tmp_0 < 65536.0) {
20368 if (tmp_0 >= 0.0) {
20369 tmp_1 = (uint16_T)tmp_0;
20370 } else {
20371 tmp_1 = 0U;
20372 }
20373 } else {
20374 tmp_1 = MAX_uint16_T;
20375 }
20376
20377 motor_Y.PWMOUT = tmp_1;
20378 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
20379 if (tmp_0 < 2.147483648E+9) {
20380 if (tmp_0 >= -2.147483648E+9) {
20381 tmp = (int32_T)tmp_0;
20382 } else {
20383 tmp = MIN_int32_T;
20384 }
20385 } else {
20386 tmp = MAX_int32_T;
20387 }
20388
20389 motor_Y.JD_Out = tmp;
20390 tmp_0 = motor_B.Motor_XHHY.KP_e;
20391 if (tmp_0 < 2.147483648E+9) {
20392 if (tmp_0 >= -2.147483648E+9) {
20393 tmp = (int32_T)tmp_0;
20394 } else {
20395 tmp = MIN_int32_T;
20396 }
20397 } else {
20398 tmp = MAX_int32_T;
20399 }
20400
20401 motor_Y.JD_Error = tmp;
20402 } else {
20403 motor_DWork.chu_jd += 0.01;
20404
20405 /* Inport: '<Root>/JD_In' */
20406 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
20407 motor_B.JD_In = motor_U.JD_In;
20408 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
20409
20410 /* Inport: '<Root>/Encode_Sp' */
20411 motor_B.Encode_Sp = motor_U.Encode_Sp;
20412
20413 /* Inport: '<Root>/System_Order' */
20414 motor_B.Slect_port = motor_U.System_Order;
20415
20416 /* Inport: '<Root>/SGWY_In' */
20417 motor_B.SGWY_In = motor_U.SGWY_In;
20418
20419 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
20420 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
20421 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
20422 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
20423 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
20424 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
20425 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
20426 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
20427 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
20428 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
20429 &motor_DWork.chu_jd);
20430
20431 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
20432 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
20433 if (tmp_0 < 65536.0) {
20434 if (tmp_0 >= 0.0) {
20435 tmp_1 = (uint16_T)tmp_0;
20436 } else {
20437 tmp_1 = 0U;
20438 }
20439 } else {
20440 tmp_1 = MAX_uint16_T;
20441 }
20442
20443 motor_Y.PWMOUT = tmp_1;
20444 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
20445 if (tmp_0 < 2.147483648E+9) {
20446 if (tmp_0 >= -2.147483648E+9) {
20447 tmp = (int32_T)tmp_0;
20448 } else {
20449 tmp = MIN_int32_T;
20450 }
20451 } else {
20452 tmp = MAX_int32_T;
20453 }
20454
20455 motor_Y.JD_Out = tmp;
20456 tmp_0 = motor_B.Motor_XHHY.KP_e;
20457 if (tmp_0 < 2.147483648E+9) {
20458 if (tmp_0 >= -2.147483648E+9) {
20459 tmp = (int32_T)tmp_0;
20460 } else {
20461 tmp = MIN_int32_T;
20462 }
20463 } else {
20464 tmp = MAX_int32_T;
20465 }
20466
20467 motor_Y.JD_Error = tmp;
20468 }
20469 break;
20470 }
20471 break;
20472
20473 default:
20474 motor_XHZY_asnq();
20475 break;
20476 }
20477}
20478
20479/* Function for Chart: '<Root>/Chart' */
20480static void motor_XHHY_n(void)
20481{
20482 uint16_T tmp;
20483 real_T tmp_0;
20484 int32_T saturatedUnaryMinus;
20485
20486 /* Inport: '<Root>/Test_Mode' */
20487 /* During 'XHHY': '<S1>:207' */
20488 if (motor_U.Test_Mode == 2) {
20489 /* Transition: '<S1>:616' */
20490 /* Transition: '<S1>:631' */
20491 motor_DWork.XiaoDaShen = 1.0;
20492
20493 /* Exit Internal 'XHHY': '<S1>:207' */
20494 if (motor_DWork.is_XHHY_n == motor_IN_Sleeping_protect_2_m) {
20495 /* Exit 'Sleeping_protect_2': '<S1>:209' */
20496 /* 关电磁制动 */
20497 motor_Y.DCZD = false;
20498 motor_DWork.is_XHHY_n = motor_IN_NO_ACTIVE_CHILD;
20499 } else {
20500 motor_DWork.is_XHHY_n = motor_IN_NO_ACTIVE_CHILD;
20501 }
20502
20503 motor_DWork.is_Algorithm_e = motor_IN_Limit_Up_Test_k;
20504
20505 /* Entry 'Limit_Up_Test': '<S1>:340' */
20506 /* 测试电子上限位 */
20507 mo_enter_internal_Limit_Up_Test();
20508 } else if (motor_U.Test_Mode == 3) {
20509 /* Transition: '<S1>:617' */
20510 /* Transition: '<S1>:634' */
20511 motor_DWork.XiaoDaShen = 1.0;
20512
20513 /* Exit Internal 'XHHY': '<S1>:207' */
20514 if (motor_DWork.is_XHHY_n == motor_IN_Sleeping_protect_2_m) {
20515 /* Exit 'Sleeping_protect_2': '<S1>:209' */
20516 /* 关电磁制动 */
20517 motor_Y.DCZD = false;
20518 motor_DWork.is_XHHY_n = motor_IN_NO_ACTIVE_CHILD;
20519 } else {
20520 motor_DWork.is_XHHY_n = motor_IN_NO_ACTIVE_CHILD;
20521 }
20522
20523 motor_DWork.is_Algorithm_e = motor_IN_Limit_Down_Test_f;
20524
20525 /* Entry 'Limit_Down_Test': '<S1>:315' */
20526 /* 电子下限位检测 */
20527 enter_internal_Limit_Down_Test();
20528 } else {
20529 switch (motor_DWork.is_XHHY_n) {
20530 case motor_IN_Defualt:
20531 /* During 'Defualt': '<S1>:208' */
20532 saturatedUnaryMinus = motor_Y.Encode_Pos;
20533 if (saturatedUnaryMinus < 0) {
20534 if (saturatedUnaryMinus <= MIN_int32_T) {
20535 saturatedUnaryMinus = MAX_int32_T;
20536 } else {
20537 saturatedUnaryMinus = -saturatedUnaryMinus;
20538 }
20539 }
20540
20541 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Pos_Zero) {
20542 /* Transition: '<S1>:637' */
20543 motor_DWork.is_XHHY_n = motor_IN_XHHY_Wait_g;
20544 motor_DWork.temporalCounter_i2 = 0U;
20545
20546 /* Entry 'XHHY_Wait': '<S1>:212' */
20547 motor_Y.Open_Result = 1U;
20548
20549 /* 开机状态位成功 */
20550 motor_DWork.In_State = 2U;
20551 motor_DWork.KG_En = 1U;
20552 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
20553 } else if (motor_DWork.temporalCounter_i2 >= (uint32_T)(15.0 /
20554 motor_DWork.Ts)) {
20555 /* Transition: '<S1>:638' */
20556 motor_DWork.is_XHHY_n = motor_IN_XHHY_Error;
20557
20558 /* Entry 'XHHY_Error': '<S1>:210' */
20559 motor_Y.Motor_En = true;
20560
20561 /* 关电机 */
20562 motor_Y.Open_Result = 2U;
20563 } else {
20564 /* Inport: '<Root>/JD_In' */
20565 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
20566 motor_B.JD_In = motor_U.JD_In;
20567 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
20568
20569 /* Inport: '<Root>/Encode_Sp' */
20570 motor_B.Encode_Sp = motor_U.Encode_Sp;
20571
20572 /* Inport: '<Root>/System_Order' */
20573 motor_B.Slect_port = motor_U.System_Order;
20574
20575 /* Inport: '<Root>/SGWY_In' */
20576 motor_B.SGWY_In = motor_U.SGWY_In;
20577
20578 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
20579 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
20580 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
20581 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
20582 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
20583 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
20584 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
20585 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
20586 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
20587 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
20588 &motor_DWork.chu_jd);
20589
20590 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
20591 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
20592 if (tmp_0 < 65536.0) {
20593 if (tmp_0 >= 0.0) {
20594 tmp = (uint16_T)tmp_0;
20595 } else {
20596 tmp = 0U;
20597 }
20598 } else {
20599 tmp = MAX_uint16_T;
20600 }
20601
20602 motor_Y.PWMOUT = tmp;
20603 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
20604 if (tmp_0 < 2.147483648E+9) {
20605 if (tmp_0 >= -2.147483648E+9) {
20606 saturatedUnaryMinus = (int32_T)tmp_0;
20607 } else {
20608 saturatedUnaryMinus = MIN_int32_T;
20609 }
20610 } else {
20611 saturatedUnaryMinus = MAX_int32_T;
20612 }
20613
20614 motor_Y.JD_Out = saturatedUnaryMinus;
20615 tmp_0 = motor_B.Motor_XHHY.KP_e;
20616 if (tmp_0 < 2.147483648E+9) {
20617 if (tmp_0 >= -2.147483648E+9) {
20618 saturatedUnaryMinus = (int32_T)tmp_0;
20619 } else {
20620 saturatedUnaryMinus = MIN_int32_T;
20621 }
20622 } else {
20623 saturatedUnaryMinus = MAX_int32_T;
20624 }
20625
20626 motor_Y.JD_Error = saturatedUnaryMinus;
20627 }
20628 break;
20629
20630 case motor_IN_Sleeping_protect_1_h:
20631 /* During 'Sleeping_protect_1': '<S1>:211' */
20632 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
20633 /* Transition: '<S1>:639' */
20634 motor_DWork.is_XHHY_n = motor_IN_Sleeping_protect_2_m;
20635
20636 /* Entry 'Sleeping_protect_2': '<S1>:209' */
20637 motor_Y.DCZD = true;
20638
20639 /* 开电磁制动 */
20640 }
20641 break;
20642
20643 case motor_IN_Sleeping_protect_2_m:
20644 /* Inport: '<Root>/System_Order' */
20645 /* During 'Sleeping_protect_2': '<S1>:209' */
20646 if (motor_U.System_Order == 5) {
20647 /* Transition: '<S1>:640' */
20648 /* Exit 'Sleeping_protect_2': '<S1>:209' */
20649 /* 关电磁制动 */
20650 motor_Y.DCZD = false;
20651 motor_DWork.is_XHHY_n = motor_IN_Sleeping_protect_3_f;
20652 motor_DWork.temporalCounter_i2 = 0U;
20653
20654 /* Entry 'Sleeping_protect_3': '<S1>:223' */
20655 motor_Y.Motor_En = false;
20656
20657 /* 开电机 */
20658 }
20659 break;
20660
20661 case motor_IN_Sleeping_protect_3_f:
20662 /* During 'Sleeping_protect_3': '<S1>:223' */
20663 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
20664 /* Transition: '<S1>:642' */
20665 motor_DWork.is_XHHY_n = motor_IN_XHHY_Run_e;
20666
20667 /* Entry 'XHHY_Run': '<S1>:213' */
20668 motor_DWork.KG_En = 1U;
20669 motor_DWork.KG_JD = 0U;
20670
20671 /* Inport: '<Root>/Showing_Angle' */
20672 motor_DWork.Showing_Angle_Last = motor_U.Showing_Angle;
20673
20674 /* Inport: '<Root>/Showing_T' */
20675 motor_DWork.Showing_T_Last = motor_U.Showing_T;
20676 }
20677 break;
20678
20679 case motor_IN_XHHY_Error:
20680 /* During 'XHHY_Error': '<S1>:210' */
20681 break;
20682
20683 case motor_IN_XHHY_Run_e:
20684 /* Inport: '<Root>/System_Order' incorporates:
20685 * Inport: '<Root>/Showing_Angle'
20686 * Inport: '<Root>/Showing_T'
20687 */
20688 /* During 'XHHY_Run': '<S1>:213' */
20689 if (motor_U.System_Order == 3) {
20690 /* Transition: '<S1>:644' */
20691 /* Transition: '<S1>:625' */
20692 moto_exit_internal_Showing_Mode();
20693 motor_DWork.is_M_Run = motor_IN_Close;
20694 motor_DWork.is_Close = motor_IN_Close_Wait;
20695 motor_DWork.temporalCounter_i1 = 0U;
20696
20697 /* Entry 'Close_Wait': '<S1>:241' */
20698 motor_Y.Open_Result = 4U;
20699
20700 /* 正在关机 */
20701 motor_Y.DCZD = false;
20702
20703 /* 解除制动 */
20704 motor_Y.Motor_En = false;
20705
20706 /* 电机使能 */
20707 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
20708 motor_DWork.EN_extern = 0U;
20709 } else if (motor_U.System_Order == 4) {
20710 /* Transition: '<S1>:641' */
20711 motor_DWork.is_XHHY_n = motor_IN_Sleeping_protect_1_h;
20712 motor_DWork.temporalCounter_i2 = 0U;
20713
20714 /* Entry 'Sleeping_protect_1': '<S1>:211' */
20715 motor_Y.Motor_En = true;
20716
20717 /* 关电机 */
20718 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
20719 } else if ((motor_DWork.Showing_Angle_Last != motor_U.Showing_Angle) ||
20720 (motor_DWork.Showing_T_Last != motor_U.Showing_T)) {
20721 /* Transition: '<S1>:947' */
20722 motor_DWork.is_XHHY_n = motor_IN_Defualt;
20723 motor_DWork.temporalCounter_i2 = 0U;
20724
20725 /* Entry 'Defualt': '<S1>:208' */
20726 /* en:KG_En=uint8(0);KG_JD=uint8(1);KG_YJ=uint8(1); */
20727 motor_DWork.KG_En = 1U;
20728 motor_DWork.KG_JD = 0U;
20729 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
20730 motor_DWork.chu_jd = 0.0;
20731 } else {
20732 /* Inport: '<Root>/Showing_Angle' */
20733 /* Simulink Function 'Showing_XHHY': '<S1>:1050' */
20734 motor_B.Showing_Angle_p = motor_U.Showing_Angle;
20735
20736 /* Inport: '<Root>/Showing_T' */
20737 motor_B.Showing_T_a = motor_U.Showing_T;
20738
20739 /* Outputs for Function Call SubSystem: '<S1>/Showing_XHHY' */
20740 motor_Showing_XHHY(motor_M, motor_B.Showing_Angle_p, motor_B.Showing_T_a,
20741 &motor_B.Showing_XHHY, (rtP_Showing_XHHY_motor *)
20742 &motor_P.Showing_XHHY, &motor_DWork.Angle_S);
20743
20744 /* End of Outputs for SubSystem: '<S1>/Showing_XHHY' */
20745 motor_DWork.chu_jd = motor_B.Showing_XHHY.DataTypeConversion1;
20746
20747 /* Inport: '<Root>/JD_In' */
20748 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
20749 motor_B.JD_In = motor_U.JD_In;
20750 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
20751
20752 /* Inport: '<Root>/Encode_Sp' */
20753 motor_B.Encode_Sp = motor_U.Encode_Sp;
20754 motor_B.Slect_port = motor_U.System_Order;
20755
20756 /* Inport: '<Root>/SGWY_In' */
20757 motor_B.SGWY_In = motor_U.SGWY_In;
20758
20759 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
20760 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
20761 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
20762 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
20763 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
20764 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
20765 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
20766 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
20767 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
20768 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
20769 &motor_DWork.chu_jd);
20770
20771 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
20772 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
20773 if (tmp_0 < 65536.0) {
20774 if (tmp_0 >= 0.0) {
20775 tmp = (uint16_T)tmp_0;
20776 } else {
20777 tmp = 0U;
20778 }
20779 } else {
20780 tmp = MAX_uint16_T;
20781 }
20782
20783 motor_Y.PWMOUT = tmp;
20784 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
20785 if (tmp_0 < 2.147483648E+9) {
20786 if (tmp_0 >= -2.147483648E+9) {
20787 saturatedUnaryMinus = (int32_T)tmp_0;
20788 } else {
20789 saturatedUnaryMinus = MIN_int32_T;
20790 }
20791 } else {
20792 saturatedUnaryMinus = MAX_int32_T;
20793 }
20794
20795 motor_Y.JD_Out = saturatedUnaryMinus;
20796 tmp_0 = motor_B.Motor_XHHY.KP_e;
20797 if (tmp_0 < 2.147483648E+9) {
20798 if (tmp_0 >= -2.147483648E+9) {
20799 saturatedUnaryMinus = (int32_T)tmp_0;
20800 } else {
20801 saturatedUnaryMinus = MIN_int32_T;
20802 }
20803 } else {
20804 saturatedUnaryMinus = MAX_int32_T;
20805 }
20806
20807 motor_Y.JD_Error = saturatedUnaryMinus;
20808 }
20809 break;
20810
20811 case motor_IN_XHHY_Run1_d:
20812 /* Inport: '<Root>/Showing_Angle' */
20813 /* During 'XHHY_Run1': '<S1>:941' */
20814 if (motor_DWork.Showing_Angle0 >= motor_U.Showing_Angle) {
20815 /* Transition: '<S1>:948' */
20816 motor_DWork.is_XHHY_n = motor_IN_XHHY_Run_e;
20817
20818 /* Entry 'XHHY_Run': '<S1>:213' */
20819 motor_DWork.KG_En = 1U;
20820 motor_DWork.KG_JD = 0U;
20821 motor_DWork.Showing_Angle_Last = motor_U.Showing_Angle;
20822
20823 /* Inport: '<Root>/Showing_T' */
20824 motor_DWork.Showing_T_Last = motor_U.Showing_T;
20825 } else {
20826 motor_DWork.Showing_Angle0 += 0.01;
20827
20828 /* Simulink Function 'Showing_XHHY': '<S1>:1050' */
20829 motor_B.Showing_Angle_p = motor_DWork.Showing_Angle0;
20830
20831 /* Inport: '<Root>/Showing_T' */
20832 motor_B.Showing_T_a = motor_U.Showing_T;
20833
20834 /* Outputs for Function Call SubSystem: '<S1>/Showing_XHHY' */
20835 motor_Showing_XHHY(motor_M, motor_B.Showing_Angle_p, motor_B.Showing_T_a,
20836 &motor_B.Showing_XHHY, (rtP_Showing_XHHY_motor *)
20837 &motor_P.Showing_XHHY, &motor_DWork.Angle_S);
20838
20839 /* End of Outputs for SubSystem: '<S1>/Showing_XHHY' */
20840 motor_DWork.chu_jd = motor_B.Showing_XHHY.DataTypeConversion1;
20841
20842 /* Inport: '<Root>/JD_In' */
20843 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
20844 motor_B.JD_In = motor_U.JD_In;
20845 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
20846
20847 /* Inport: '<Root>/Encode_Sp' */
20848 motor_B.Encode_Sp = motor_U.Encode_Sp;
20849
20850 /* Inport: '<Root>/System_Order' */
20851 motor_B.Slect_port = motor_U.System_Order;
20852
20853 /* Inport: '<Root>/SGWY_In' */
20854 motor_B.SGWY_In = motor_U.SGWY_In;
20855
20856 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
20857 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
20858 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
20859 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
20860 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
20861 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
20862 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
20863 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
20864 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
20865 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
20866 &motor_DWork.chu_jd);
20867
20868 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
20869 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
20870 if (tmp_0 < 65536.0) {
20871 if (tmp_0 >= 0.0) {
20872 tmp = (uint16_T)tmp_0;
20873 } else {
20874 tmp = 0U;
20875 }
20876 } else {
20877 tmp = MAX_uint16_T;
20878 }
20879
20880 motor_Y.PWMOUT = tmp;
20881 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
20882 if (tmp_0 < 2.147483648E+9) {
20883 if (tmp_0 >= -2.147483648E+9) {
20884 saturatedUnaryMinus = (int32_T)tmp_0;
20885 } else {
20886 saturatedUnaryMinus = MIN_int32_T;
20887 }
20888 } else {
20889 saturatedUnaryMinus = MAX_int32_T;
20890 }
20891
20892 motor_Y.JD_Out = saturatedUnaryMinus;
20893 tmp_0 = motor_B.Motor_XHHY.KP_e;
20894 if (tmp_0 < 2.147483648E+9) {
20895 if (tmp_0 >= -2.147483648E+9) {
20896 saturatedUnaryMinus = (int32_T)tmp_0;
20897 } else {
20898 saturatedUnaryMinus = MIN_int32_T;
20899 }
20900 } else {
20901 saturatedUnaryMinus = MAX_int32_T;
20902 }
20903
20904 motor_Y.JD_Error = saturatedUnaryMinus;
20905 }
20906 break;
20907
20908 default:
20909 /* During 'XHHY_Wait': '<S1>:212' */
20910 if (motor_DWork.temporalCounter_i2 >= 100U) {
20911 /* Transition: '<S1>:643' */
20912 motor_DWork.is_XHHY_n = motor_IN_XHHY_Run1_d;
20913
20914 /* Entry 'XHHY_Run1': '<S1>:941' */
20915 motor_DWork.KG_JD = 0U;
20916 motor_DWork.Showing_Angle0 = 0.0;
20917 } else {
20918 /* Inport: '<Root>/JD_In' */
20919 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
20920 motor_B.JD_In = motor_U.JD_In;
20921 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
20922
20923 /* Inport: '<Root>/Encode_Sp' */
20924 motor_B.Encode_Sp = motor_U.Encode_Sp;
20925
20926 /* Inport: '<Root>/System_Order' */
20927 motor_B.Slect_port = motor_U.System_Order;
20928
20929 /* Inport: '<Root>/SGWY_In' */
20930 motor_B.SGWY_In = motor_U.SGWY_In;
20931
20932 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
20933 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
20934 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
20935 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
20936 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
20937 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
20938 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
20939 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
20940 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
20941 &motor_DWork.P_KP, &motor_DWork.V_KI, &motor_DWork.V_KP,
20942 &motor_DWork.chu_jd);
20943
20944 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
20945 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
20946 if (tmp_0 < 65536.0) {
20947 if (tmp_0 >= 0.0) {
20948 tmp = (uint16_T)tmp_0;
20949 } else {
20950 tmp = 0U;
20951 }
20952 } else {
20953 tmp = MAX_uint16_T;
20954 }
20955
20956 motor_Y.PWMOUT = tmp;
20957 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
20958 if (tmp_0 < 2.147483648E+9) {
20959 if (tmp_0 >= -2.147483648E+9) {
20960 saturatedUnaryMinus = (int32_T)tmp_0;
20961 } else {
20962 saturatedUnaryMinus = MIN_int32_T;
20963 }
20964 } else {
20965 saturatedUnaryMinus = MAX_int32_T;
20966 }
20967
20968 motor_Y.JD_Out = saturatedUnaryMinus;
20969 tmp_0 = motor_B.Motor_XHHY.KP_e;
20970 if (tmp_0 < 2.147483648E+9) {
20971 if (tmp_0 >= -2.147483648E+9) {
20972 saturatedUnaryMinus = (int32_T)tmp_0;
20973 } else {
20974 saturatedUnaryMinus = MIN_int32_T;
20975 }
20976 } else {
20977 saturatedUnaryMinus = MAX_int32_T;
20978 }
20979
20980 motor_Y.JD_Error = saturatedUnaryMinus;
20981
20982 /* PWMOUT=2500; */
20983 }
20984 break;
20985 }
20986 }
20987
20988 /* End of Inport: '<Root>/Test_Mode' */
20989}
20990
20991/* Function for Chart: '<Root>/Chart' */
20992static void motor_Algorithm_ac(void)
20993{
20994 real_T tmp;
20995 int32_T saturatedUnaryMinus;
20996
20997 /* During 'Algorithm': '<S1>:205' */
20998 switch (motor_DWork.is_Algorithm_e) {
20999 case motor_IN_Defualt:
21000 /* Inport: '<Root>/Motor_Num' */
21001 /* During 'Defualt': '<S1>:206' */
21002 if (motor_U.Motor_Num == 3) {
21003 /* Transition: '<S1>:615' */
21004 motor_DWork.is_Algorithm_e = motor_IN_XHHY_m;
21005
21006 /* Entry Internal 'XHHY': '<S1>:207' */
21007 /* Transition: '<S1>:636' */
21008 motor_DWork.is_XHHY_n = motor_IN_Defualt;
21009 motor_DWork.temporalCounter_i2 = 0U;
21010
21011 /* Entry 'Defualt': '<S1>:208' */
21012 /* en:KG_En=uint8(0);KG_JD=uint8(1);KG_YJ=uint8(1); */
21013 motor_DWork.KG_En = 1U;
21014 motor_DWork.KG_JD = 0U;
21015 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
21016 motor_DWork.chu_jd = 0.0;
21017 } else if (motor_U.Motor_Num == 2) {
21018 /* Transition: '<S1>:619' */
21019 motor_DWork.is_Algorithm_e = motor_IN_HY;
21020
21021 /* Entry Internal 'HY': '<S1>:214' */
21022 /* Transition: '<S1>:645' */
21023 motor_DWork.is_HY_e = motor_IN_Defualt1;
21024 motor_DWork.temporalCounter_i2 = 0U;
21025
21026 /* Entry 'Defualt1': '<S1>:215' */
21027 motor_DWork.KG_JD = 0U;
21028 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
21029 motor_DWork.chu_jd = 0.0;
21030 motor_DWork.Saturation_limit_speed = 5000.0;
21031 } else {
21032 if (motor_U.Motor_Num == 1) {
21033 /* Transition: '<S1>:621' */
21034 motor_DWork.is_Algorithm_e = motor_IN_XHZY_p;
21035
21036 /* Entry Internal 'XHZY': '<S1>:220' */
21037 /* Transition: '<S1>:654' */
21038 motor_DWork.is_XHZY_h = motor_IN_Defualt;
21039 motor_DWork.temporalCounter_i2 = 0U;
21040
21041 /* Entry 'Defualt': '<S1>:221' */
21042 /* en:KG_En=uint8(0);KG_JD=uint8(1);KG_YJ=uint8(1); */
21043 motor_DWork.KG_En = 1U;
21044 motor_DWork.KG_JD = 0U;
21045 motor_DWork.KG_YJ = 0U;
21046 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
21047 motor_DWork.chu_jd = 0.0;
21048 }
21049 }
21050
21051 /* End of Inport: '<Root>/Motor_Num' */
21052 break;
21053
21054 case motor_IN_HY:
21055 /* Inport: '<Root>/Test_Mode' */
21056 /* During 'HY': '<S1>:214' */
21057 if (motor_U.Test_Mode == 2) {
21058 /* Transition: '<S1>:622' */
21059 /* Transition: '<S1>:631' */
21060 motor_DWork.XiaoDaShen = 1.0;
21061
21062 /* Exit Internal 'HY': '<S1>:214' */
21063 if (motor_DWork.is_HY_e == motor_IN_Sleeping_protect_2_m) {
21064 /* Exit 'Sleeping_protect_2': '<S1>:179' */
21065 /* 关电磁制动 */
21066 motor_Y.DCZD = false;
21067 motor_DWork.is_HY_e = motor_IN_NO_ACTIVE_CHILD;
21068 } else {
21069 motor_DWork.is_HY_e = motor_IN_NO_ACTIVE_CHILD;
21070 }
21071
21072 motor_DWork.is_Algorithm_e = motor_IN_Limit_Up_Test_k;
21073
21074 /* Entry 'Limit_Up_Test': '<S1>:340' */
21075 /* 测试电子上限位 */
21076 mo_enter_internal_Limit_Up_Test();
21077 } else if (motor_U.Test_Mode == 3) {
21078 /* Transition: '<S1>:624' */
21079 /* Transition: '<S1>:634' */
21080 motor_DWork.XiaoDaShen = 1.0;
21081
21082 /* Exit Internal 'HY': '<S1>:214' */
21083 if (motor_DWork.is_HY_e == motor_IN_Sleeping_protect_2_m) {
21084 /* Exit 'Sleeping_protect_2': '<S1>:179' */
21085 /* 关电磁制动 */
21086 motor_Y.DCZD = false;
21087 motor_DWork.is_HY_e = motor_IN_NO_ACTIVE_CHILD;
21088 } else {
21089 motor_DWork.is_HY_e = motor_IN_NO_ACTIVE_CHILD;
21090 }
21091
21092 motor_DWork.is_Algorithm_e = motor_IN_Limit_Down_Test_f;
21093
21094 /* Entry 'Limit_Down_Test': '<S1>:315' */
21095 /* 电子下限位检测 */
21096 enter_internal_Limit_Down_Test();
21097 } else {
21098 switch (motor_DWork.is_HY_e) {
21099 case motor_IN_Defualt1:
21100 /* During 'Defualt1': '<S1>:215' */
21101 saturatedUnaryMinus = motor_Y.Encode_Pos;
21102 if (saturatedUnaryMinus < 0) {
21103 if (saturatedUnaryMinus <= MIN_int32_T) {
21104 saturatedUnaryMinus = MAX_int32_T;
21105 } else {
21106 saturatedUnaryMinus = -saturatedUnaryMinus;
21107 }
21108 }
21109
21110 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Pos_Zero) {
21111 /* Transition: '<S1>:647' */
21112 motor_DWork.is_HY_e = motor_IN_XHHY_Wait_g;
21113 motor_DWork.temporalCounter_i2 = 0U;
21114
21115 /* Entry 'XHHY_Wait': '<S1>:218' */
21116 motor_Y.Open_Result = 1U;
21117
21118 /* 开机状态位成功 */
21119 motor_DWork.In_State = 2U;
21120 } else if (motor_DWork.temporalCounter_i2 >= (uint32_T)(15.0 /
21121 motor_DWork.Ts)) {
21122 /* Transition: '<S1>:648' */
21123 motor_DWork.is_HY_e = motor_IN_XHHY_Error;
21124
21125 /* Entry 'XHHY_Error': '<S1>:217' */
21126 motor_Y.Motor_En = true;
21127
21128 /* 关电机 */
21129 motor_Y.Open_Result = 2U;
21130 } else {
21131 /* Inport: '<Root>/JD_In' */
21132 /* Simulink Function 'Motor_HYDG1': '<S1>:115' */
21133 motor_B.JD_In_n = motor_U.JD_In;
21134 motor_B.Encode_Pos_cm = motor_Y.Encode_Pos;
21135
21136 /* Inport: '<Root>/System_Order' */
21137 motor_B.Slect_port_e = motor_U.System_Order;
21138
21139 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG1' */
21140 motor_Motor_HYDG1(motor_M, motor_B.JD_In_n, motor_B.Encode_Pos_cm,
21141 motor_B.Slect_port_e, &motor_B.Motor_HYDG1,
21142 &motor_DWork.Motor_HYDG1, (rtP_Motor_HYDG1_motor *)
21143 &motor_P.Motor_HYDG1, &motor_DWork.JD_HYDG,
21144 &motor_DWork.JD_HYDG_HC, &motor_DWork.KG_JD,
21145 &motor_DWork.KG_clc, &motor_DWork.P_KP,
21146 &motor_DWork.Saturation_limit_speed,
21147 &motor_DWork.V_KI, &motor_DWork.V_KP,
21148 &motor_DWork.chu_jd);
21149
21150 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG1' */
21151 motor_Y.PWMOUT = motor_B.Motor_HYDG1.DataTypeConversion3;
21152 tmp = motor_B.Motor_HYDG1.KP_e;
21153 if (tmp < 2.147483648E+9) {
21154 if (tmp >= -2.147483648E+9) {
21155 saturatedUnaryMinus = (int32_T)tmp;
21156 } else {
21157 saturatedUnaryMinus = MIN_int32_T;
21158 }
21159 } else {
21160 saturatedUnaryMinus = MAX_int32_T;
21161 }
21162
21163 motor_Y.JD_Error = saturatedUnaryMinus;
21164 tmp = motor_B.Motor_HYDG1.KP_JD;
21165 if (tmp < 2.147483648E+9) {
21166 if (tmp >= -2.147483648E+9) {
21167 saturatedUnaryMinus = (int32_T)tmp;
21168 } else {
21169 saturatedUnaryMinus = MIN_int32_T;
21170 }
21171 } else {
21172 saturatedUnaryMinus = MAX_int32_T;
21173 }
21174
21175 motor_Y.JD_Out = saturatedUnaryMinus;
21176 }
21177 break;
21178
21179 case motor_IN_Sleeping_protect_1_h:
21180 /* During 'Sleeping_protect_1': '<S1>:229' */
21181 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts))
21182 {
21183 /* Transition: '<S1>:646' */
21184 motor_DWork.is_HY_e = motor_IN_Sleeping_protect_2_m;
21185
21186 /* Entry 'Sleeping_protect_2': '<S1>:179' */
21187 motor_Y.DCZD = true;
21188
21189 /* 开电磁制动 */
21190 }
21191 break;
21192
21193 case motor_IN_Sleeping_protect_2_m:
21194 /* Inport: '<Root>/System_Order' */
21195 /* During 'Sleeping_protect_2': '<S1>:179' */
21196 if (motor_U.System_Order == 5) {
21197 /* Transition: '<S1>:649' */
21198 /* Exit 'Sleeping_protect_2': '<S1>:179' */
21199 /* 关电磁制动 */
21200 motor_Y.DCZD = false;
21201 motor_DWork.is_HY_e = motor_IN_Sleeping_protect_3_f;
21202 motor_DWork.temporalCounter_i2 = 0U;
21203
21204 /* Entry 'Sleeping_protect_3': '<S1>:158' */
21205 motor_Y.Motor_En = false;
21206
21207 /* 开电机 */
21208 }
21209 break;
21210
21211 case motor_IN_Sleeping_protect_3_f:
21212 /* During 'Sleeping_protect_3': '<S1>:158' */
21213 if (motor_DWork.temporalCounter_i2 >= (uint32_T)(0.5 / motor_DWork.Ts))
21214 {
21215 /* Transition: '<S1>:652' */
21216 motor_DWork.is_HY_e = motor_IN_Defualt1;
21217 motor_DWork.temporalCounter_i2 = 0U;
21218
21219 /* Entry 'Defualt1': '<S1>:215' */
21220 motor_DWork.KG_JD = 0U;
21221 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
21222 motor_DWork.chu_jd = 0.0;
21223 motor_DWork.Saturation_limit_speed = 5000.0;
21224 }
21225 break;
21226
21227 case motor_IN_XHHY_Error:
21228 /* During 'XHHY_Error': '<S1>:217' */
21229 break;
21230
21231 case motor_IN_XHHY_Run_e:
21232 /* Inport: '<Root>/System_Order' incorporates:
21233 * Inport: '<Root>/Showing_Angle'
21234 * Inport: '<Root>/Showing_T'
21235 */
21236 /* During 'XHHY_Run': '<S1>:219' */
21237 if (motor_U.System_Order == 3) {
21238 /* Transition: '<S1>:651' */
21239 /* Transition: '<S1>:625' */
21240 moto_exit_internal_Showing_Mode();
21241 motor_DWork.is_M_Run = motor_IN_Close;
21242 motor_DWork.is_Close = motor_IN_Close_Wait;
21243 motor_DWork.temporalCounter_i1 = 0U;
21244
21245 /* Entry 'Close_Wait': '<S1>:241' */
21246 motor_Y.Open_Result = 4U;
21247
21248 /* 正在关机 */
21249 motor_Y.DCZD = false;
21250
21251 /* 解除制动 */
21252 motor_Y.Motor_En = false;
21253
21254 /* 电机使能 */
21255 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
21256 motor_DWork.EN_extern = 0U;
21257 } else if (motor_U.System_Order == 4) {
21258 /* Transition: '<S1>:650' */
21259 motor_DWork.is_HY_e = motor_IN_Sleeping_protect_1_h;
21260 motor_DWork.temporalCounter_i2 = 0U;
21261
21262 /* Entry 'Sleeping_protect_1': '<S1>:229' */
21263 motor_Y.Motor_En = true;
21264
21265 /* 关电机 */
21266 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
21267 } else if ((motor_DWork.Showing_Angle_Last != motor_U.Showing_Angle) ||
21268 (motor_DWork.Showing_T_Last != motor_U.Showing_T)) {
21269 /* Transition: '<S1>:937' */
21270 motor_DWork.is_HY_e = motor_IN_Defualt1;
21271 motor_DWork.temporalCounter_i2 = 0U;
21272
21273 /* Entry 'Defualt1': '<S1>:215' */
21274 motor_DWork.KG_JD = 0U;
21275 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
21276 motor_DWork.chu_jd = 0.0;
21277 motor_DWork.Saturation_limit_speed = 5000.0;
21278 } else {
21279 /* Inport: '<Root>/Showing_Angle' */
21280 /* [PWMOUT,JD_Error,JD_Out] = Motor_HYDG1(JD_In,Encode_Pos,System_Order): */
21281 /* Simulink Function 'Showing': '<S1>:180' */
21282 motor_B.Showing_Angle = motor_U.Showing_Angle;
21283
21284 /* Inport: '<Root>/Showing_T' */
21285 motor_B.Showing_T = motor_U.Showing_T;
21286
21287 /* Outputs for Function Call SubSystem: '<S1>/Showing' */
21288 motor_Showing(motor_M, motor_B.Showing_Angle, motor_B.Showing_T,
21289 &motor_B.Showing, (rtP_Showing_motor *)&motor_P.Showing,
21290 &motor_DWork.Angle_S);
21291
21292 /* End of Outputs for SubSystem: '<S1>/Showing' */
21293 motor_DWork.chu_jd = motor_B.Showing.DataTypeConversion1;
21294
21295 /* Inport: '<Root>/JD_In' */
21296 /* Simulink Function 'Motor_HYDG1': '<S1>:115' */
21297 motor_B.JD_In_n = motor_U.JD_In;
21298 motor_B.Encode_Pos_cm = motor_Y.Encode_Pos;
21299 motor_B.Slect_port_e = motor_U.System_Order;
21300
21301 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG1' */
21302 motor_Motor_HYDG1(motor_M, motor_B.JD_In_n, motor_B.Encode_Pos_cm,
21303 motor_B.Slect_port_e, &motor_B.Motor_HYDG1,
21304 &motor_DWork.Motor_HYDG1, (rtP_Motor_HYDG1_motor *)
21305 &motor_P.Motor_HYDG1, &motor_DWork.JD_HYDG,
21306 &motor_DWork.JD_HYDG_HC, &motor_DWork.KG_JD,
21307 &motor_DWork.KG_clc, &motor_DWork.P_KP,
21308 &motor_DWork.Saturation_limit_speed,
21309 &motor_DWork.V_KI, &motor_DWork.V_KP,
21310 &motor_DWork.chu_jd);
21311
21312 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG1' */
21313 motor_Y.PWMOUT = motor_B.Motor_HYDG1.DataTypeConversion3;
21314 tmp = motor_B.Motor_HYDG1.KP_e;
21315 if (tmp < 2.147483648E+9) {
21316 if (tmp >= -2.147483648E+9) {
21317 saturatedUnaryMinus = (int32_T)tmp;
21318 } else {
21319 saturatedUnaryMinus = MIN_int32_T;
21320 }
21321 } else {
21322 saturatedUnaryMinus = MAX_int32_T;
21323 }
21324
21325 motor_Y.JD_Error = saturatedUnaryMinus;
21326 tmp = motor_B.Motor_HYDG1.KP_JD;
21327 if (tmp < 2.147483648E+9) {
21328 if (tmp >= -2.147483648E+9) {
21329 saturatedUnaryMinus = (int32_T)tmp;
21330 } else {
21331 saturatedUnaryMinus = MIN_int32_T;
21332 }
21333 } else {
21334 saturatedUnaryMinus = MAX_int32_T;
21335 }
21336
21337 motor_Y.JD_Out = saturatedUnaryMinus;
21338 }
21339 break;
21340
21341 case motor_IN_XHHY_Run1_d:
21342 /* Inport: '<Root>/Showing_Angle' */
21343 /* During 'XHHY_Run1': '<S1>:935' */
21344 if (motor_DWork.Showing_Angle0 >= motor_U.Showing_Angle) {
21345 /* Transition: '<S1>:936' */
21346 motor_DWork.is_HY_e = motor_IN_XHHY_Run_e;
21347
21348 /* Entry 'XHHY_Run': '<S1>:219' */
21349 motor_DWork.Showing_Angle_Last = motor_U.Showing_Angle;
21350
21351 /* Inport: '<Root>/Showing_T' */
21352 motor_DWork.Showing_T_Last = motor_U.Showing_T;
21353 } else {
21354 motor_DWork.Showing_Angle0 += 0.01;
21355
21356 /* Simulink Function 'Showing': '<S1>:180' */
21357 motor_B.Showing_Angle = motor_DWork.Showing_Angle0;
21358
21359 /* Inport: '<Root>/Showing_T' */
21360 motor_B.Showing_T = motor_U.Showing_T;
21361
21362 /* Outputs for Function Call SubSystem: '<S1>/Showing' */
21363 motor_Showing(motor_M, motor_B.Showing_Angle, motor_B.Showing_T,
21364 &motor_B.Showing, (rtP_Showing_motor *)&motor_P.Showing,
21365 &motor_DWork.Angle_S);
21366
21367 /* End of Outputs for SubSystem: '<S1>/Showing' */
21368 motor_DWork.chu_jd = motor_B.Showing.DataTypeConversion1;
21369
21370 /* Inport: '<Root>/JD_In' */
21371 /* Simulink Function 'Motor_HYDG1': '<S1>:115' */
21372 motor_B.JD_In_n = motor_U.JD_In;
21373 motor_B.Encode_Pos_cm = motor_Y.Encode_Pos;
21374
21375 /* Inport: '<Root>/System_Order' */
21376 motor_B.Slect_port_e = motor_U.System_Order;
21377
21378 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG1' */
21379 motor_Motor_HYDG1(motor_M, motor_B.JD_In_n, motor_B.Encode_Pos_cm,
21380 motor_B.Slect_port_e, &motor_B.Motor_HYDG1,
21381 &motor_DWork.Motor_HYDG1, (rtP_Motor_HYDG1_motor *)
21382 &motor_P.Motor_HYDG1, &motor_DWork.JD_HYDG,
21383 &motor_DWork.JD_HYDG_HC, &motor_DWork.KG_JD,
21384 &motor_DWork.KG_clc, &motor_DWork.P_KP,
21385 &motor_DWork.Saturation_limit_speed,
21386 &motor_DWork.V_KI, &motor_DWork.V_KP,
21387 &motor_DWork.chu_jd);
21388
21389 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG1' */
21390 motor_Y.PWMOUT = motor_B.Motor_HYDG1.DataTypeConversion3;
21391 tmp = motor_B.Motor_HYDG1.KP_e;
21392 if (tmp < 2.147483648E+9) {
21393 if (tmp >= -2.147483648E+9) {
21394 saturatedUnaryMinus = (int32_T)tmp;
21395 } else {
21396 saturatedUnaryMinus = MIN_int32_T;
21397 }
21398 } else {
21399 saturatedUnaryMinus = MAX_int32_T;
21400 }
21401
21402 motor_Y.JD_Error = saturatedUnaryMinus;
21403 tmp = motor_B.Motor_HYDG1.KP_JD;
21404 if (tmp < 2.147483648E+9) {
21405 if (tmp >= -2.147483648E+9) {
21406 saturatedUnaryMinus = (int32_T)tmp;
21407 } else {
21408 saturatedUnaryMinus = MIN_int32_T;
21409 }
21410 } else {
21411 saturatedUnaryMinus = MAX_int32_T;
21412 }
21413
21414 motor_Y.JD_Out = saturatedUnaryMinus;
21415 }
21416 break;
21417
21418 default:
21419 /* During 'XHHY_Wait': '<S1>:218' */
21420 if (motor_DWork.temporalCounter_i2 >= 100U) {
21421 /* Transition: '<S1>:653' */
21422 motor_DWork.is_HY_e = motor_IN_XHHY_Run1_d;
21423
21424 /* Entry 'XHHY_Run1': '<S1>:935' */
21425 motor_DWork.KG_JD = 0U;
21426 motor_DWork.Showing_Angle0 = 0.0;
21427 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
21428 motor_DWork.Rate_limit_speed = 50000.0;
21429 motor_DWork.Saturation_limit_speed = 16000.0;
21430 } else {
21431 /* Inport: '<Root>/JD_In' */
21432 /* Simulink Function 'Motor_HYDG1': '<S1>:115' */
21433 motor_B.JD_In_n = motor_U.JD_In;
21434 motor_B.Encode_Pos_cm = motor_Y.Encode_Pos;
21435
21436 /* Inport: '<Root>/System_Order' */
21437 motor_B.Slect_port_e = motor_U.System_Order;
21438
21439 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG1' */
21440 motor_Motor_HYDG1(motor_M, motor_B.JD_In_n, motor_B.Encode_Pos_cm,
21441 motor_B.Slect_port_e, &motor_B.Motor_HYDG1,
21442 &motor_DWork.Motor_HYDG1, (rtP_Motor_HYDG1_motor *)
21443 &motor_P.Motor_HYDG1, &motor_DWork.JD_HYDG,
21444 &motor_DWork.JD_HYDG_HC, &motor_DWork.KG_JD,
21445 &motor_DWork.KG_clc, &motor_DWork.P_KP,
21446 &motor_DWork.Saturation_limit_speed,
21447 &motor_DWork.V_KI, &motor_DWork.V_KP,
21448 &motor_DWork.chu_jd);
21449
21450 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG1' */
21451 motor_Y.PWMOUT = motor_B.Motor_HYDG1.DataTypeConversion3;
21452 tmp = motor_B.Motor_HYDG1.KP_e;
21453 if (tmp < 2.147483648E+9) {
21454 if (tmp >= -2.147483648E+9) {
21455 saturatedUnaryMinus = (int32_T)tmp;
21456 } else {
21457 saturatedUnaryMinus = MIN_int32_T;
21458 }
21459 } else {
21460 saturatedUnaryMinus = MAX_int32_T;
21461 }
21462
21463 motor_Y.JD_Error = saturatedUnaryMinus;
21464 tmp = motor_B.Motor_HYDG1.KP_JD;
21465 if (tmp < 2.147483648E+9) {
21466 if (tmp >= -2.147483648E+9) {
21467 saturatedUnaryMinus = (int32_T)tmp;
21468 } else {
21469 saturatedUnaryMinus = MIN_int32_T;
21470 }
21471 } else {
21472 saturatedUnaryMinus = MAX_int32_T;
21473 }
21474
21475 motor_Y.JD_Out = saturatedUnaryMinus;
21476
21477 /* PWMOUT=2500; */
21478 }
21479 break;
21480 }
21481 }
21482
21483 /* End of Inport: '<Root>/Test_Mode' */
21484 break;
21485
21486 case motor_IN_Limit_Down_Test_f:
21487 motor_Limit_Down_Test_b();
21488 break;
21489
21490 case motor_IN_Limit_Up_Test_k:
21491 motor_Limit_Up_Test_a();
21492 break;
21493
21494 case motor_IN_XHHY_m:
21495 motor_XHHY_n();
21496 break;
21497
21498 default:
21499 motor_XHZY();
21500 break;
21501 }
21502}
21503
21504/* Function for Chart: '<Root>/Chart' */
21505static void motor_Showing_Mode(void)
21506{
21507 /* During 'Showing_Mode': '<S1>:51' */
21508 if (motor_DWork.is_Showing_Mode == motor_IN_RUN) {
21509 /* During 'RUN': '<S1>:186' */
21510 motor_Algorithm_ac();
21511 if (motor_DWork.is_Showing_Mode == motor_IN_RUN) {
21512 /* During 'Limit_Check': '<S1>:200' */
21513 switch (motor_DWork.is_Limit_Check_j) {
21514 case motor_IN_defult_bq:
21515 /* During 'defult': '<S1>:202' */
21516 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts))
21517 {
21518 /* Transition: '<S1>:607' */
21519 motor_DWork.is_Limit_Check_j = motor_IN_p1_aqc;
21520
21521 /* Entry 'p1': '<S1>:201' */
21522 motor_Y.Flag_Down_limit = 1U;
21523 motor_Y.Flag_Up_limit = 1U;
21524
21525 /* Flag_Up_GZ_limit=1;
21526 Flag_Down_GZ_limit=1; */
21527 }
21528 break;
21529
21530 case motor_IN_p1_aqc:
21531 /* Inport: '<Root>/Up_Limit' incorporates:
21532 * Inport: '<Root>/Down_Limit'
21533 */
21534 /* During 'p1': '<S1>:201' */
21535 if ((motor_U.Up_Limit == 0) && (motor_DWork.XiaoDaShen == 0.0)) {
21536 /* Transition: '<S1>:608' */
21537 motor_DWork.is_Limit_Check_j = motor_IN_p2_kb;
21538
21539 /* Entry 'p2': '<S1>:203' */
21540 motor_Y.Flag_Up_limit = 0U;
21541 } else {
21542 if ((motor_U.Down_Limit == 0) && (motor_DWork.XiaoDaShen == 0.0)) {
21543 /* Transition: '<S1>:610' */
21544 motor_DWork.is_Limit_Check_j = motor_IN_p3_ex;
21545
21546 /* Entry 'p3': '<S1>:204' */
21547 motor_Y.Flag_Down_limit = 0U;
21548 }
21549 }
21550 break;
21551
21552 case motor_IN_p2_kb:
21553 /* Inport: '<Root>/Up_Limit' */
21554 /* During 'p2': '<S1>:203' */
21555 if (motor_U.Up_Limit == 1) {
21556 /* Transition: '<S1>:609' */
21557 motor_DWork.is_Limit_Check_j = motor_IN_p1_aqc;
21558
21559 /* Entry 'p1': '<S1>:201' */
21560 motor_Y.Flag_Down_limit = 1U;
21561 motor_Y.Flag_Up_limit = 1U;
21562
21563 /* Flag_Up_GZ_limit=1;
21564 Flag_Down_GZ_limit=1; */
21565 }
21566 break;
21567
21568 default:
21569 /* Inport: '<Root>/Down_Limit' */
21570 /* During 'p3': '<S1>:204' */
21571 if (motor_U.Down_Limit == 1) {
21572 /* Transition: '<S1>:611' */
21573 motor_DWork.is_Limit_Check_j = motor_IN_p1_aqc;
21574
21575 /* Entry 'p1': '<S1>:201' */
21576 motor_Y.Flag_Down_limit = 1U;
21577 motor_Y.Flag_Up_limit = 1U;
21578
21579 /* Flag_Up_GZ_limit=1;
21580 Flag_Down_GZ_limit=1; */
21581 }
21582 break;
21583 }
21584 }
21585 } else {
21586 /* During 'Start': '<S1>:226' */
21587 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(1.0 / motor_DWork.Ts)) {
21588 /* Transition: '<S1>:438' */
21589 motor_DWork.is_Showing_Mode = motor_IN_RUN;
21590
21591 /* Entry Internal 'RUN': '<S1>:186' */
21592 /* Transition: '<S1>:588' */
21593 motor_DWork.is_active_Algorithm_b = 1U;
21594
21595 /* Entry Internal 'Algorithm': '<S1>:205' */
21596 /* Transition: '<S1>:614' */
21597 motor_DWork.is_Algorithm_e = motor_IN_Defualt;
21598
21599 /* Entry 'Defualt': '<S1>:206' */
21600 motor_DWork.chu_jd = 0.0;
21601 motor_DWork.KG = 0U;
21602 motor_Y.Motor_En = false;
21603 motor_Y.DCZD = false;
21604
21605 /* 开电磁制动,低有效 */
21606 motor_DWork.is_active_Limit_Check_o = 1U;
21607
21608 /* Entry Internal 'Limit_Check': '<S1>:200' */
21609 /* Transition: '<S1>:606' */
21610 motor_DWork.is_Limit_Check_j = motor_IN_defult_bq;
21611 motor_DWork.temporalCounter_i1 = 0U;
21612 }
21613 }
21614}
21615
21616/* Function for Chart: '<Root>/Chart' */
21617static void motor_Algorithm(void)
21618{
21619 uint16_T tmp;
21620 real_T tmp_0;
21621 int32_T saturatedUnaryMinus;
21622
21623 /* During 'Algorithm': '<S1>:131' */
21624 switch (motor_DWork.is_Algorithm) {
21625 case motor_IN_HYDG_Close:
21626 /* During 'HYDG_Close': '<S1>:132' */
21627 switch (motor_DWork.is_HYDG_Close) {
21628 case motor_IN_Defualt:
21629 /* During 'Defualt': '<S1>:134' */
21630 saturatedUnaryMinus = motor_Y.Encode_Pos;
21631 if (saturatedUnaryMinus < 0) {
21632 if (saturatedUnaryMinus <= MIN_int32_T) {
21633 saturatedUnaryMinus = MAX_int32_T;
21634 } else {
21635 saturatedUnaryMinus = -saturatedUnaryMinus;
21636 }
21637 }
21638
21639 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Pos_Zero) {
21640 /* Transition: '<S1>:512' */
21641 motor_DWork.is_HYDG_Close = motor_IN_XHZY_Close1;
21642 motor_DWork.temporalCounter_i1 = 0U;
21643
21644 /* Entry 'XHZY_Close1': '<S1>:310' */
21645 motor_Y.Motor_En = true;
21646
21647 /* 电机失能 */
21648 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
21649 } else if (motor_DWork.temporalCounter_i1 >= (uint32_T)(2.0 /
21650 motor_DWork.Ts)) {
21651 /* Transition: '<S1>:513' */
21652 /* 等待2s,查看关机是否成功 */
21653 motor_DWork.is_HYDG_Close = motor_IN_XHZY_Error;
21654
21655 /* Entry 'XHZY_Error': '<S1>:133' */
21656 motor_Y.Motor_En = true;
21657
21658 /* 电机失能 */
21659 } else {
21660 /* Inport: '<Root>/JD_In' */
21661 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
21662 motor_B.JD_In_f = motor_U.JD_In;
21663 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
21664
21665 /* Inport: '<Root>/System_Order' */
21666 motor_B.Slect_port_c = motor_U.System_Order;
21667
21668 /* Inport: '<Root>/Encode_Sp' */
21669 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
21670
21671 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
21672 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
21673 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
21674 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
21675 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
21676 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
21677 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
21678 &motor_DWork.chu_jd);
21679
21680 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
21681 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
21682 tmp_0 = motor_B.Motor_HYDG.KP_e;
21683 if (tmp_0 < 2.147483648E+9) {
21684 if (tmp_0 >= -2.147483648E+9) {
21685 saturatedUnaryMinus = (int32_T)tmp_0;
21686 } else {
21687 saturatedUnaryMinus = MIN_int32_T;
21688 }
21689 } else {
21690 saturatedUnaryMinus = MAX_int32_T;
21691 }
21692
21693 motor_Y.JD_Error = saturatedUnaryMinus;
21694 tmp_0 = motor_B.Motor_HYDG.KP_JD;
21695 if (tmp_0 < 2.147483648E+9) {
21696 if (tmp_0 >= -2.147483648E+9) {
21697 saturatedUnaryMinus = (int32_T)tmp_0;
21698 } else {
21699 saturatedUnaryMinus = MIN_int32_T;
21700 }
21701 } else {
21702 saturatedUnaryMinus = MAX_int32_T;
21703 }
21704
21705 motor_Y.JD_Out = saturatedUnaryMinus;
21706 }
21707 break;
21708
21709 case motor_IN_Int_k:
21710 /* During 'Int': '<S1>:312' */
21711 if (motor_DWork.chu_jd < 0.0) {
21712 /* Transition: '<S1>:507' */
21713 motor_DWork.is_HYDG_Close = motor_IN_Int1_d;
21714
21715 /* Entry 'Int1': '<S1>:313' */
21716 motor_DWork.chu_jd += 0.01;
21717
21718 /* Inport: '<Root>/JD_In' */
21719 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
21720 motor_B.JD_In_f = motor_U.JD_In;
21721 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
21722
21723 /* Inport: '<Root>/System_Order' */
21724 motor_B.Slect_port_c = motor_U.System_Order;
21725
21726 /* Inport: '<Root>/Encode_Sp' */
21727 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
21728
21729 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
21730 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
21731 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
21732 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
21733 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
21734 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
21735 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
21736 &motor_DWork.chu_jd);
21737
21738 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
21739 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
21740 tmp_0 = motor_B.Motor_HYDG.KP_e;
21741 if (tmp_0 < 2.147483648E+9) {
21742 if (tmp_0 >= -2.147483648E+9) {
21743 saturatedUnaryMinus = (int32_T)tmp_0;
21744 } else {
21745 saturatedUnaryMinus = MIN_int32_T;
21746 }
21747 } else {
21748 saturatedUnaryMinus = MAX_int32_T;
21749 }
21750
21751 motor_Y.JD_Error = saturatedUnaryMinus;
21752 tmp_0 = motor_B.Motor_HYDG.KP_JD;
21753 if (tmp_0 < 2.147483648E+9) {
21754 if (tmp_0 >= -2.147483648E+9) {
21755 saturatedUnaryMinus = (int32_T)tmp_0;
21756 } else {
21757 saturatedUnaryMinus = MIN_int32_T;
21758 }
21759 } else {
21760 saturatedUnaryMinus = MAX_int32_T;
21761 }
21762
21763 motor_Y.JD_Out = saturatedUnaryMinus;
21764 } else if (motor_DWork.chu_jd > 0.0) {
21765 /* Transition: '<S1>:508' */
21766 motor_DWork.is_HYDG_Close = motor_IN_Int2;
21767
21768 /* Entry 'Int2': '<S1>:314' */
21769 motor_DWork.chu_jd -= 0.01;
21770
21771 /* Inport: '<Root>/JD_In' */
21772 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
21773 motor_B.JD_In_f = motor_U.JD_In;
21774 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
21775
21776 /* Inport: '<Root>/System_Order' */
21777 motor_B.Slect_port_c = motor_U.System_Order;
21778
21779 /* Inport: '<Root>/Encode_Sp' */
21780 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
21781
21782 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
21783 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
21784 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
21785 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
21786 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
21787 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
21788 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
21789 &motor_DWork.chu_jd);
21790
21791 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
21792 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
21793 tmp_0 = motor_B.Motor_HYDG.KP_e;
21794 if (tmp_0 < 2.147483648E+9) {
21795 if (tmp_0 >= -2.147483648E+9) {
21796 saturatedUnaryMinus = (int32_T)tmp_0;
21797 } else {
21798 saturatedUnaryMinus = MIN_int32_T;
21799 }
21800 } else {
21801 saturatedUnaryMinus = MAX_int32_T;
21802 }
21803
21804 motor_Y.JD_Error = saturatedUnaryMinus;
21805 tmp_0 = motor_B.Motor_HYDG.KP_JD;
21806 if (tmp_0 < 2.147483648E+9) {
21807 if (tmp_0 >= -2.147483648E+9) {
21808 saturatedUnaryMinus = (int32_T)tmp_0;
21809 } else {
21810 saturatedUnaryMinus = MIN_int32_T;
21811 }
21812 } else {
21813 saturatedUnaryMinus = MAX_int32_T;
21814 }
21815
21816 motor_Y.JD_Out = saturatedUnaryMinus;
21817 } else {
21818 if (motor_DWork.chu_jd == 0.0) {
21819 /* Transition: '<S1>:509' */
21820 motor_DWork.is_HYDG_Close = motor_IN_Defualt;
21821 motor_DWork.temporalCounter_i1 = 0U;
21822
21823 /* Entry 'Defualt': '<S1>:134' */
21824 motor_DWork.chu_jd = 0.0;
21825 }
21826 }
21827 break;
21828
21829 case motor_IN_Int1_d:
21830 /* During 'Int1': '<S1>:313' */
21831 if (fabs(motor_DWork.chu_jd) < 0.1) {
21832 /* Transition: '<S1>:510' */
21833 motor_DWork.is_HYDG_Close = motor_IN_Defualt;
21834 motor_DWork.temporalCounter_i1 = 0U;
21835
21836 /* Entry 'Defualt': '<S1>:134' */
21837 motor_DWork.chu_jd = 0.0;
21838 } else {
21839 motor_DWork.chu_jd += 0.01;
21840
21841 /* Inport: '<Root>/JD_In' */
21842 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
21843 motor_B.JD_In_f = motor_U.JD_In;
21844 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
21845
21846 /* Inport: '<Root>/System_Order' */
21847 motor_B.Slect_port_c = motor_U.System_Order;
21848
21849 /* Inport: '<Root>/Encode_Sp' */
21850 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
21851
21852 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
21853 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
21854 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
21855 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
21856 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
21857 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
21858 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
21859 &motor_DWork.chu_jd);
21860
21861 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
21862 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
21863 tmp_0 = motor_B.Motor_HYDG.KP_e;
21864 if (tmp_0 < 2.147483648E+9) {
21865 if (tmp_0 >= -2.147483648E+9) {
21866 saturatedUnaryMinus = (int32_T)tmp_0;
21867 } else {
21868 saturatedUnaryMinus = MIN_int32_T;
21869 }
21870 } else {
21871 saturatedUnaryMinus = MAX_int32_T;
21872 }
21873
21874 motor_Y.JD_Error = saturatedUnaryMinus;
21875 tmp_0 = motor_B.Motor_HYDG.KP_JD;
21876 if (tmp_0 < 2.147483648E+9) {
21877 if (tmp_0 >= -2.147483648E+9) {
21878 saturatedUnaryMinus = (int32_T)tmp_0;
21879 } else {
21880 saturatedUnaryMinus = MIN_int32_T;
21881 }
21882 } else {
21883 saturatedUnaryMinus = MAX_int32_T;
21884 }
21885
21886 motor_Y.JD_Out = saturatedUnaryMinus;
21887 }
21888 break;
21889
21890 case motor_IN_Int2:
21891 /* During 'Int2': '<S1>:314' */
21892 if (fabs(motor_DWork.chu_jd) < 0.1) {
21893 /* Transition: '<S1>:511' */
21894 motor_DWork.is_HYDG_Close = motor_IN_Defualt;
21895 motor_DWork.temporalCounter_i1 = 0U;
21896
21897 /* Entry 'Defualt': '<S1>:134' */
21898 motor_DWork.chu_jd = 0.0;
21899 } else {
21900 motor_DWork.chu_jd -= 0.01;
21901
21902 /* Inport: '<Root>/JD_In' */
21903 /* Simulink Function 'Motor_HYDG': '<S1>:77' */
21904 motor_B.JD_In_f = motor_U.JD_In;
21905 motor_B.Encode_Pos_i = motor_Y.Encode_Pos;
21906
21907 /* Inport: '<Root>/System_Order' */
21908 motor_B.Slect_port_c = motor_U.System_Order;
21909
21910 /* Inport: '<Root>/Encode_Sp' */
21911 motor_B.Encode_Sp_n = motor_U.Encode_Sp;
21912
21913 /* Outputs for Function Call SubSystem: '<S1>/Motor_HYDG' */
21914 motor_Motor_HYDG(motor_M, motor_B.JD_In_f, motor_B.Encode_Pos_i,
21915 motor_B.Slect_port_c, motor_B.Encode_Sp_n,
21916 &motor_B.Motor_HYDG, &motor_DWork.Motor_HYDG,
21917 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG,
21918 &motor_DWork.JD_HYDG, &motor_DWork.JD_HYDG_HC,
21919 &motor_DWork.KG_JD, &motor_DWork.KG_clc,
21920 &motor_DWork.chu_jd);
21921
21922 /* End of Outputs for SubSystem: '<S1>/Motor_HYDG' */
21923 motor_Y.PWMOUT = motor_B.Motor_HYDG.DataTypeConversion3;
21924 tmp_0 = motor_B.Motor_HYDG.KP_e;
21925 if (tmp_0 < 2.147483648E+9) {
21926 if (tmp_0 >= -2.147483648E+9) {
21927 saturatedUnaryMinus = (int32_T)tmp_0;
21928 } else {
21929 saturatedUnaryMinus = MIN_int32_T;
21930 }
21931 } else {
21932 saturatedUnaryMinus = MAX_int32_T;
21933 }
21934
21935 motor_Y.JD_Error = saturatedUnaryMinus;
21936 tmp_0 = motor_B.Motor_HYDG.KP_JD;
21937 if (tmp_0 < 2.147483648E+9) {
21938 if (tmp_0 >= -2.147483648E+9) {
21939 saturatedUnaryMinus = (int32_T)tmp_0;
21940 } else {
21941 saturatedUnaryMinus = MIN_int32_T;
21942 }
21943 } else {
21944 saturatedUnaryMinus = MAX_int32_T;
21945 }
21946
21947 motor_Y.JD_Out = saturatedUnaryMinus;
21948 }
21949 break;
21950
21951 case motor_IN_XHZY_Close_c:
21952 /* During 'XHZY_Close': '<S1>:135' */
21953 break;
21954
21955 case motor_IN_XHZY_Close1:
21956 /* During 'XHZY_Close1': '<S1>:310' */
21957 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
21958 /* Transition: '<S1>:515' */
21959 motor_DWork.is_HYDG_Close = motor_IN_XHZY_Close_c;
21960
21961 /* Entry 'XHZY_Close': '<S1>:135' */
21962 motor_Y.Open_Result = 2U;
21963
21964 /* 关机成功 */
21965 motor_DWork.In_State = 3U;
21966 motor_Y.DCZD = true;
21967
21968 /* 电池只懂引脚拉高,关电磁制动,低有效 */
21969 }
21970 break;
21971
21972 default:
21973 /* During 'XHZY_Error': '<S1>:133' */
21974 /* Transition: '<S1>:514' */
21975 motor_DWork.is_HYDG_Close = motor_IN_XHZY_Close1;
21976 motor_DWork.temporalCounter_i1 = 0U;
21977
21978 /* Entry 'XHZY_Close1': '<S1>:310' */
21979 motor_Y.Motor_En = true;
21980
21981 /* 电机失能 */
21982 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
21983 break;
21984 }
21985 break;
21986
21987 case motor_IN_XHHY_Close:
21988 /* Inport: '<Root>/JD_In' */
21989 /* During 'XHHY_Close': '<S1>:136' */
21990 /* Simulink Function 'Motor_XHHY': '<S1>:42' */
21991 motor_B.JD_In = motor_U.JD_In;
21992 motor_B.Encode_Pos_c = motor_Y.Encode_Pos;
21993
21994 /* Inport: '<Root>/Encode_Sp' */
21995 motor_B.Encode_Sp = motor_U.Encode_Sp;
21996
21997 /* Inport: '<Root>/System_Order' */
21998 motor_B.Slect_port = motor_U.System_Order;
21999
22000 /* Inport: '<Root>/SGWY_In' */
22001 motor_B.SGWY_In = motor_U.SGWY_In;
22002
22003 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHHY' */
22004 motor_Motor_XHHY(motor_M, motor_B.JD_In, motor_B.Encode_Pos_c,
22005 motor_B.Encode_Sp, motor_B.Slect_port, motor_B.SGWY_In,
22006 &motor_B.Motor_XHHY, &motor_DWork.Motor_XHHY,
22007 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY,
22008 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
22009 &motor_DWork.EN_extern, &motor_DWork.JD_XHHY,
22010 &motor_DWork.JD_XHHY_HC, &motor_DWork.KG_En,
22011 &motor_DWork.KG_JD, &motor_DWork.KG_clc, &motor_DWork.P_KP,
22012 &motor_DWork.V_KI, &motor_DWork.V_KP, &motor_DWork.chu_jd);
22013
22014 /* End of Outputs for SubSystem: '<S1>/Motor_XHHY' */
22015 tmp_0 = motor_B.Motor_XHHY.ZXF_PWM;
22016 if (tmp_0 < 65536.0) {
22017 if (tmp_0 >= 0.0) {
22018 tmp = (uint16_T)tmp_0;
22019 } else {
22020 tmp = 0U;
22021 }
22022 } else {
22023 tmp = MAX_uint16_T;
22024 }
22025
22026 motor_Y.PWMOUT = tmp;
22027 tmp_0 = motor_B.Motor_XHHY.KP_JD1;
22028 if (tmp_0 < 2.147483648E+9) {
22029 if (tmp_0 >= -2.147483648E+9) {
22030 saturatedUnaryMinus = (int32_T)tmp_0;
22031 } else {
22032 saturatedUnaryMinus = MIN_int32_T;
22033 }
22034 } else {
22035 saturatedUnaryMinus = MAX_int32_T;
22036 }
22037
22038 motor_Y.JD_Out = saturatedUnaryMinus;
22039 tmp_0 = motor_B.Motor_XHHY.KP_e;
22040 if (tmp_0 < 2.147483648E+9) {
22041 if (tmp_0 >= -2.147483648E+9) {
22042 saturatedUnaryMinus = (int32_T)tmp_0;
22043 } else {
22044 saturatedUnaryMinus = MIN_int32_T;
22045 }
22046 } else {
22047 saturatedUnaryMinus = MAX_int32_T;
22048 }
22049
22050 motor_Y.JD_Error = saturatedUnaryMinus;
22051 switch (motor_DWork.is_XHHY_Close) {
22052 case motor_IN_Defualt:
22053 /* During 'Defualt': '<S1>:138' */
22054 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(10.0 / motor_DWork.Ts)) {
22055 /* Transition: '<S1>:517' */
22056 /* 等待10s,查看关机是否成功 */
22057 motor_DWork.is_XHHY_Close = motor_IN_XHZY_Error_f;
22058
22059 /* Entry 'XHZY_Error': '<S1>:137' */
22060 motor_Y.Motor_En = true;
22061
22062 /* 电机失能 */
22063 motor_Y.Open_Result = 1U;
22064 } else {
22065 saturatedUnaryMinus = motor_Y.Encode_Pos;
22066 if (saturatedUnaryMinus < 0) {
22067 if (saturatedUnaryMinus <= MIN_int32_T) {
22068 saturatedUnaryMinus = MAX_int32_T;
22069 } else {
22070 saturatedUnaryMinus = -saturatedUnaryMinus;
22071 }
22072 }
22073
22074 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Pos_Zero) {
22075 /* Transition: '<S1>:518' */
22076 motor_DWork.is_XHHY_Close = motor_IN_XHZY_Close1_f;
22077 motor_DWork.temporalCounter_i1 = 0U;
22078
22079 /* Entry 'XHZY_Close1': '<S1>:311' */
22080 motor_Y.Motor_En = true;
22081
22082 /* 电机失能 */
22083 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
22084 }
22085 }
22086 break;
22087
22088 case motor_IN_XHZY_Close_cm:
22089 /* During 'XHZY_Close': '<S1>:139' */
22090 break;
22091
22092 case motor_IN_XHZY_Close1_f:
22093 /* During 'XHZY_Close1': '<S1>:311' */
22094 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
22095 /* Transition: '<S1>:520' */
22096 motor_DWork.is_XHHY_Close = motor_IN_XHZY_Close_cm;
22097
22098 /* Entry 'XHZY_Close': '<S1>:139' */
22099 motor_Y.Open_Result = 2U;
22100
22101 /* 关机成功 */
22102 motor_DWork.In_State = 3U;
22103 motor_Y.DCZD = true;
22104
22105 /* 电池只懂引脚拉高,关电磁制动,低有效 */
22106 }
22107 break;
22108
22109 default:
22110 /* During 'XHZY_Error': '<S1>:137' */
22111 /* Transition: '<S1>:519' */
22112 motor_DWork.is_XHHY_Close = motor_IN_XHZY_Close1_f;
22113 motor_DWork.temporalCounter_i1 = 0U;
22114
22115 /* Entry 'XHZY_Close1': '<S1>:311' */
22116 motor_Y.Motor_En = true;
22117
22118 /* 电机失能 */
22119 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
22120 break;
22121 }
22122 break;
22123
22124 default:
22125 /* Inport: '<Root>/JD_In' */
22126 /* During 'XHZY_Close': '<S1>:140' */
22127 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
22128 motor_B.JD_In_d = motor_U.JD_In;
22129 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
22130
22131 /* Inport: '<Root>/YJ_In' */
22132 motor_B.YJ_In = motor_U.YJ_In;
22133
22134 /* Inport: '<Root>/Encode_Sp' */
22135 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
22136
22137 /* Inport: '<Root>/System_Order' */
22138 motor_B.Slect_port_h = motor_U.System_Order;
22139
22140 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
22141 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
22142 motor_B.YJ_In, motor_B.Encode_Sp_l, motor_B.Slect_port_h,
22143 &motor_B.Motor_XHZY, &motor_DWork.Motor_XHZY,
22144 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY,
22145 &motor_DWork.Angle_extern, &motor_DWork.Angle_internal,
22146 &motor_DWork.EN_extern, &motor_DWork.Forword,
22147 &motor_DWork.JD_XHZY, &motor_DWork.JD_XHZY_HC,
22148 &motor_DWork.KG_En, &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
22149 &motor_DWork.KG_clc, &motor_DWork.P_KP, &motor_DWork.V_KI,
22150 &motor_DWork.V_KP, &motor_DWork.YJ_XHZY,
22151 &motor_DWork.YJ_XHZY_HC, &motor_DWork.chu_jd);
22152
22153 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
22154 tmp_0 = motor_B.Motor_XHZY.ZXF_PWM;
22155 if (tmp_0 < 65536.0) {
22156 if (tmp_0 >= 0.0) {
22157 tmp = (uint16_T)tmp_0;
22158 } else {
22159 tmp = 0U;
22160 }
22161 } else {
22162 tmp = MAX_uint16_T;
22163 }
22164
22165 motor_Y.PWMOUT = tmp;
22166 tmp_0 = motor_B.Motor_XHZY.KP_JD;
22167 if (tmp_0 < 2.147483648E+9) {
22168 if (tmp_0 >= -2.147483648E+9) {
22169 saturatedUnaryMinus = (int32_T)tmp_0;
22170 } else {
22171 saturatedUnaryMinus = MIN_int32_T;
22172 }
22173 } else {
22174 saturatedUnaryMinus = MAX_int32_T;
22175 }
22176
22177 motor_Y.JD_Out = saturatedUnaryMinus;
22178 tmp_0 = motor_B.Motor_XHZY.KP_e;
22179 if (tmp_0 < 2.147483648E+9) {
22180 if (tmp_0 >= -2.147483648E+9) {
22181 saturatedUnaryMinus = (int32_T)tmp_0;
22182 } else {
22183 saturatedUnaryMinus = MIN_int32_T;
22184 }
22185 } else {
22186 saturatedUnaryMinus = MAX_int32_T;
22187 }
22188
22189 motor_Y.JD_Error = saturatedUnaryMinus;
22190 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
22191 switch (motor_DWork.is_XHZY_Close) {
22192 case motor_IN_Defualt:
22193 /* During 'Defualt': '<S1>:75' */
22194 saturatedUnaryMinus = motor_Y.Encode_Pos;
22195 if (saturatedUnaryMinus < 0) {
22196 if (saturatedUnaryMinus <= MIN_int32_T) {
22197 saturatedUnaryMinus = MAX_int32_T;
22198 } else {
22199 saturatedUnaryMinus = -saturatedUnaryMinus;
22200 }
22201 }
22202
22203 if ((uint32_T)saturatedUnaryMinus <= motor_DWork.Encode_Pos_Zero) {
22204 /* Transition: '<S1>:524' */
22205 motor_DWork.is_XHZY_Close = motor_IN_XHZY_Close_cm;
22206 motor_DWork.temporalCounter_i1 = 0U;
22207
22208 /* Entry 'XHZY_Close': '<S1>:36' */
22209 motor_Y.Motor_En = true;
22210
22211 /* 电机失能 */
22212 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
22213 } else {
22214 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(10.0 / motor_DWork.Ts))
22215 {
22216 /* Transition: '<S1>:521' */
22217 /* 等待10s,查看关机是否成功 */
22218 motor_DWork.is_XHZY_Close = motor_IN_XHZY_Error_f;
22219
22220 /* Entry 'XHZY_Error': '<S1>:74' */
22221 motor_Y.Motor_En = true;
22222
22223 /* 电机失能 */
22224 motor_Y.Open_Result = 1U;
22225 }
22226 }
22227 break;
22228
22229 case motor_IN_XHZY_Close_cm:
22230 /* During 'XHZY_Close': '<S1>:36' */
22231 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
22232 /* Transition: '<S1>:525' */
22233 motor_DWork.is_XHZY_Close = motor_IN_XHZY_Close1_f;
22234
22235 /* Entry 'XHZY_Close1': '<S1>:309' */
22236 motor_Y.Open_Result = 2U;
22237
22238 /* 关机成功 */
22239 motor_DWork.In_State = 3U;
22240 motor_Y.DCZD = true;
22241
22242 /* 电池只懂引脚拉高,关电磁制动,低有效 */
22243 }
22244 break;
22245
22246 case motor_IN_XHZY_Close1_f:
22247 /* During 'XHZY_Close1': '<S1>:309' */
22248 break;
22249
22250 default:
22251 /* During 'XHZY_Error': '<S1>:74' */
22252 /* Transition: '<S1>:522' */
22253 motor_DWork.is_XHZY_Close = motor_IN_XHZY_Close_cm;
22254 motor_DWork.temporalCounter_i1 = 0U;
22255
22256 /* Entry 'XHZY_Close': '<S1>:36' */
22257 motor_Y.Motor_En = true;
22258
22259 /* 电机失能 */
22260 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
22261 break;
22262 }
22263 break;
22264 }
22265}
22266
22267/* Function for Chart: '<Root>/Chart' */
22268static void motor_M_Run(void)
22269{
22270 boolean_T guard1 = false;
22271 uint16_T tmp;
22272 int32_T tmp_0;
22273 real_T tmp_1;
22274
22275 /* Inport: '<Root>/Encode_Sp' */
22276 /* During 'M_Run': '<S1>:11' */
22277 motor_DWork.Encode_Pos0 += (real_T)motor_U.Encode_Sp;
22278
22279 /* Inport: '<Root>/Encode_Pos_Zero_Average' */
22280 tmp_1 = motor_DWork.Encode_Pos0 - (real_T)motor_U.Encode_Pos_Zero_Average *
22281 motor_DWork.Average_En;
22282 if (tmp_1 < 2.147483648E+9) {
22283 if (tmp_1 >= -2.147483648E+9) {
22284 tmp_0 = (int32_T)tmp_1;
22285 } else {
22286 tmp_0 = MIN_int32_T;
22287 }
22288 } else {
22289 tmp_0 = MAX_int32_T;
22290 }
22291
22292 motor_Y.Encode_Pos = tmp_0;
22293 switch (motor_DWork.is_M_Run) {
22294 case motor_IN_Close:
22295 /* Inport: '<Root>/System_Order' */
22296 /* During 'Close': '<S1>:130' */
22297 if (motor_U.System_Order == 2) {
22298 /* Inport: '<Root>/Working_Mode' */
22299 /* Transition: '<S1>:386' */
22300 if (motor_U.Working_Mode == 3) {
22301 /* Transition: '<S1>:387' */
22302 /* Exit Internal 'Close': '<S1>:130' */
22303 /* Exit Internal 'Algorithm': '<S1>:131' */
22304 /* Exit Internal 'HYDG_Close': '<S1>:132' */
22305 motor_DWork.is_HYDG_Close = motor_IN_NO_ACTIVE_CHILD;
22306 motor_DWork.is_Algorithm = motor_IN_NO_ACTIVE_CHILD;
22307
22308 /* Exit Internal 'XHHY_Close': '<S1>:136' */
22309 motor_DWork.is_XHHY_Close = motor_IN_NO_ACTIVE_CHILD;
22310
22311 /* Exit Internal 'XHZY_Close': '<S1>:140' */
22312 motor_DWork.is_XHZY_Close = motor_IN_NO_ACTIVE_CHILD;
22313 motor_DWork.is_Close = motor_IN_NO_ACTIVE_CHILD;
22314 motor_DWork.is_M_Run = motor_IN_Normal_Mode;
22315
22316 /* Entry 'Normal_Mode': '<S1>:13' */
22317 /* 正常模式 */
22318 motor_Y.DCZD = false;
22319
22320 /* 开电磁制动,低有效 */
22321 motor_Y.Open_Result = 3U;
22322
22323 /* 开机未完成状态位 */
22324 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
22325
22326 /* Entry Internal 'Normal_Mode': '<S1>:13' */
22327 /* Transition: '<S1>:410' */
22328 motor_DWork.is_Normal_Mode = motor_IN_defult1;
22329 motor_DWork.temporalCounter_i1 = 0U;
22330 } else if (motor_U.Working_Mode == 2) {
22331 /* Transition: '<S1>:385' */
22332 /* Exit Internal 'Close': '<S1>:130' */
22333 /* Exit Internal 'Algorithm': '<S1>:131' */
22334 /* Exit Internal 'HYDG_Close': '<S1>:132' */
22335 motor_DWork.is_HYDG_Close = motor_IN_NO_ACTIVE_CHILD;
22336 motor_DWork.is_Algorithm = motor_IN_NO_ACTIVE_CHILD;
22337
22338 /* Exit Internal 'XHHY_Close': '<S1>:136' */
22339 motor_DWork.is_XHHY_Close = motor_IN_NO_ACTIVE_CHILD;
22340
22341 /* Exit Internal 'XHZY_Close': '<S1>:140' */
22342 motor_DWork.is_XHZY_Close = motor_IN_NO_ACTIVE_CHILD;
22343 motor_DWork.is_Close = motor_IN_NO_ACTIVE_CHILD;
22344 motor_DWork.is_M_Run = motor_IN_Showing_Mode;
22345
22346 /* Entry 'Showing_Mode': '<S1>:51' */
22347 /* 检视模式 */
22348 motor_Y.DCZD = false;
22349
22350 /* 开电磁制动,低有效 */
22351 motor_Y.Open_Result = 3U;
22352
22353 /* 开机未完成状态位 */
22354 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
22355 motor_DWork.KG = 0U;
22356
22357 /* Entry Internal 'Showing_Mode': '<S1>:51' */
22358 /* Transition: '<S1>:437' */
22359 motor_DWork.is_Showing_Mode = motor_IN_Start;
22360 motor_DWork.temporalCounter_i1 = 0U;
22361 } else if (motor_U.Working_Mode == 1) {
22362 /* Transition: '<S1>:382' */
22363 /* Exit Internal 'Close': '<S1>:130' */
22364 /* Exit Internal 'Algorithm': '<S1>:131' */
22365 /* Exit Internal 'HYDG_Close': '<S1>:132' */
22366 motor_DWork.is_HYDG_Close = motor_IN_NO_ACTIVE_CHILD;
22367 motor_DWork.is_Algorithm = motor_IN_NO_ACTIVE_CHILD;
22368
22369 /* Exit Internal 'XHHY_Close': '<S1>:136' */
22370 motor_DWork.is_XHHY_Close = motor_IN_NO_ACTIVE_CHILD;
22371
22372 /* Exit Internal 'XHZY_Close': '<S1>:140' */
22373 motor_DWork.is_XHZY_Close = motor_IN_NO_ACTIVE_CHILD;
22374 motor_DWork.is_Close = motor_IN_NO_ACTIVE_CHILD;
22375 motor_DWork.is_M_Run = motor_IN_Test_Mode;
22376
22377 /* Entry 'Test_Mode': '<S1>:35' */
22378 /* 调试模式 */
22379 /* Entry Internal 'Test_Mode': '<S1>:35' */
22380 /* Transition: '<S1>:424' */
22381 motor_DWork.is_Test_Mode = motor_IN_defult;
22382
22383 /* Entry 'defult': '<S1>:239' */
22384 motor_Y.Open_Result = 1U;
22385
22386 /* 开机状态位成功 */
22387 motor_DWork.In_State = 2U;
22388 } else {
22389 guard1 = true;
22390 }
22391
22392 /* End of Inport: '<Root>/Working_Mode' */
22393 } else {
22394 guard1 = true;
22395 }
22396 break;
22397
22398 case motor_IN_Initialize:
22399 motor_Initialize();
22400 break;
22401
22402 case motor_IN_Normal_Mode:
22403 motor_Normal_Mode();
22404 break;
22405
22406 case motor_IN_Showing_Mode:
22407 motor_Showing_Mode();
22408 break;
22409
22410 default:
22411 /* Inport: '<Root>/System_Order' */
22412 /* During 'Test_Mode': '<S1>:35' */
22413 if (motor_U.System_Order == 3) {
22414 /* Transition: '<S1>:393' */
22415 motor_exit_internal_Test_Mode();
22416 motor_DWork.is_M_Run = motor_IN_Close;
22417 motor_DWork.is_Close = motor_IN_Close_Wait;
22418 motor_DWork.temporalCounter_i1 = 0U;
22419
22420 /* Entry 'Close_Wait': '<S1>:241' */
22421 motor_Y.Open_Result = 4U;
22422
22423 /* 正在关机 */
22424 motor_Y.DCZD = false;
22425
22426 /* 解除制动 */
22427 motor_Y.Motor_En = false;
22428
22429 /* 电机使能 */
22430 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
22431 motor_DWork.EN_extern = 0U;
22432 } else {
22433 switch (motor_DWork.is_Test_Mode) {
22434 case motor_IN_Calibration:
22435 /* Inport: '<Root>/Test_Mode' */
22436 /* During 'Calibration': '<S1>:237' */
22437 if (motor_U.Test_Mode != 1) {
22438 /* Transition: '<S1>:426' */
22439 motor_Y.DCZD = true;
22440 motor_DWork.is_Test_Mode = motor_IN_defult;
22441
22442 /* Entry 'defult': '<S1>:239' */
22443 motor_Y.Open_Result = 1U;
22444
22445 /* 开机状态位成功 */
22446 motor_DWork.In_State = 2U;
22447 }
22448 break;
22449
22450 case motor_IN_Dspace_Test:
22451 /* During 'Dspace_Test': '<S1>:1051' */
22452 switch (motor_DWork.is_Dspace_Test) {
22453 case motor_IN_Int:
22454 /* During 'Int': '<S1>:1058' */
22455 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts))
22456 {
22457 /* Transition: '<S1>:1056' */
22458 motor_DWork.is_Dspace_Test = motor_IN_Int1;
22459 motor_DWork.temporalCounter_i1 = 0U;
22460 }
22461 break;
22462
22463 case motor_IN_Int1:
22464 /* During 'Int1': '<S1>:1060' */
22465 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(3.0 / motor_DWork.Ts))
22466 {
22467 /* Transition: '<S1>:1057' */
22468 motor_DWork.is_Dspace_Test = motor_IN_Int4;
22469 motor_DWork.temporalCounter_i1 = 0U;
22470
22471 /* Entry 'Int4': '<S1>:1061' */
22472 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
22473 } else {
22474 /* Inport: '<Root>/JD_In' */
22475 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
22476 motor_B.JD_In_d = motor_U.JD_In;
22477 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
22478
22479 /* Inport: '<Root>/YJ_In' */
22480 motor_B.YJ_In = motor_U.YJ_In;
22481
22482 /* Inport: '<Root>/Encode_Sp' */
22483 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
22484 motor_B.Slect_port_h = motor_U.System_Order;
22485
22486 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
22487 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
22488 motor_B.YJ_In, motor_B.Encode_Sp_l,
22489 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
22490 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
22491 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
22492 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
22493 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
22494 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
22495 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
22496 &motor_DWork.KG_clc, &motor_DWork.P_KP,
22497 &motor_DWork.V_KI, &motor_DWork.V_KP,
22498 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
22499 &motor_DWork.chu_jd);
22500
22501 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
22502 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
22503 if (tmp_1 < 65536.0) {
22504 if (tmp_1 >= 0.0) {
22505 tmp = (uint16_T)tmp_1;
22506 } else {
22507 tmp = 0U;
22508 }
22509 } else {
22510 tmp = MAX_uint16_T;
22511 }
22512
22513 motor_Y.PWMOUT = tmp;
22514 tmp_1 = motor_B.Motor_XHZY.KP_JD;
22515 if (tmp_1 < 2.147483648E+9) {
22516 if (tmp_1 >= -2.147483648E+9) {
22517 tmp_0 = (int32_T)tmp_1;
22518 } else {
22519 tmp_0 = MIN_int32_T;
22520 }
22521 } else {
22522 tmp_0 = MAX_int32_T;
22523 }
22524
22525 motor_Y.JD_Out = tmp_0;
22526 tmp_1 = motor_B.Motor_XHZY.KP_e;
22527 if (tmp_1 < 2.147483648E+9) {
22528 if (tmp_1 >= -2.147483648E+9) {
22529 tmp_0 = (int32_T)tmp_1;
22530 } else {
22531 tmp_0 = MIN_int32_T;
22532 }
22533 } else {
22534 tmp_0 = MAX_int32_T;
22535 }
22536
22537 motor_Y.JD_Error = tmp_0;
22538 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
22539 }
22540 break;
22541
22542 case motor_IN_Int3:
22543 /* During 'Int3': '<S1>:1059' */
22544 /* Transition: '<S1>:1053' */
22545 motor_DWork.is_Dspace_Test = motor_IN_NO_ACTIVE_CHILD;
22546 motor_DWork.is_Test_Mode = motor_IN_defult;
22547
22548 /* Entry 'defult': '<S1>:239' */
22549 motor_Y.Open_Result = 1U;
22550
22551 /* 开机状态位成功 */
22552 motor_DWork.In_State = 2U;
22553 break;
22554
22555 case motor_IN_Int4:
22556 /* During 'Int4': '<S1>:1061' */
22557 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts))
22558 {
22559 /* Transition: '<S1>:1066' */
22560 motor_DWork.is_Dspace_Test = motor_IN_Int5;
22561
22562 /* Entry 'Int5': '<S1>:1065' */
22563 motor_Y.dSpace_Init = 1U;
22564 }
22565 break;
22566
22567 case motor_IN_Int5:
22568 /* Inport: '<Root>/Test_Mode' */
22569 /* During 'Int5': '<S1>:1065' */
22570 if (motor_U.Test_Mode != 5) {
22571 /* Transition: '<S1>:1055' */
22572 motor_DWork.is_Dspace_Test = motor_IN_Int6;
22573 motor_DWork.temporalCounter_i1 = 0U;
22574
22575 /* Entry 'Int6': '<S1>:1067' */
22576 motor_Y.dSpace_Init = 0U;
22577 }
22578 break;
22579
22580 default:
22581 /* During 'Int6': '<S1>:1067' */
22582 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(3.0 / motor_DWork.Ts))
22583 {
22584 /* Transition: '<S1>:1068' */
22585 motor_DWork.is_Dspace_Test = motor_IN_Int3;
22586
22587 /* Entry 'Int3': '<S1>:1059' */
22588 motor_Y.Motor_En = true;
22589
22590 /* 电机失能 */
22591 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
22592 } else {
22593 /* Inport: '<Root>/JD_In' */
22594 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
22595 motor_B.JD_In_d = motor_U.JD_In;
22596 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
22597
22598 /* Inport: '<Root>/YJ_In' */
22599 motor_B.YJ_In = motor_U.YJ_In;
22600
22601 /* Inport: '<Root>/Encode_Sp' */
22602 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
22603 motor_B.Slect_port_h = motor_U.System_Order;
22604
22605 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
22606 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
22607 motor_B.YJ_In, motor_B.Encode_Sp_l,
22608 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
22609 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
22610 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
22611 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
22612 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
22613 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
22614 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
22615 &motor_DWork.KG_clc, &motor_DWork.P_KP,
22616 &motor_DWork.V_KI, &motor_DWork.V_KP,
22617 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
22618 &motor_DWork.chu_jd);
22619
22620 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
22621 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
22622 if (tmp_1 < 65536.0) {
22623 if (tmp_1 >= 0.0) {
22624 tmp = (uint16_T)tmp_1;
22625 } else {
22626 tmp = 0U;
22627 }
22628 } else {
22629 tmp = MAX_uint16_T;
22630 }
22631
22632 motor_Y.PWMOUT = tmp;
22633 tmp_1 = motor_B.Motor_XHZY.KP_JD;
22634 if (tmp_1 < 2.147483648E+9) {
22635 if (tmp_1 >= -2.147483648E+9) {
22636 tmp_0 = (int32_T)tmp_1;
22637 } else {
22638 tmp_0 = MIN_int32_T;
22639 }
22640 } else {
22641 tmp_0 = MAX_int32_T;
22642 }
22643
22644 motor_Y.JD_Out = tmp_0;
22645 tmp_1 = motor_B.Motor_XHZY.KP_e;
22646 if (tmp_1 < 2.147483648E+9) {
22647 if (tmp_1 >= -2.147483648E+9) {
22648 tmp_0 = (int32_T)tmp_1;
22649 } else {
22650 tmp_0 = MIN_int32_T;
22651 }
22652 } else {
22653 tmp_0 = MAX_int32_T;
22654 }
22655
22656 motor_Y.JD_Error = tmp_0;
22657 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
22658 }
22659 break;
22660 }
22661 break;
22662
22663 case motor_IN_Elevation_Test:
22664 /* During 'Elevation_Test': '<S1>:304' */
22665 switch (motor_DWork.is_Elevation_Test) {
22666 case motor_IN_Int:
22667 /* During 'Int': '<S1>:305' */
22668 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts))
22669 {
22670 /* Transition: '<S1>:736' */
22671 motor_DWork.is_Elevation_Test = motor_IN_Int1;
22672
22673 /* Inport: '<Root>/JD_In' */
22674 /* Entry 'Int1': '<S1>:306' */
22675 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
22676 motor_B.JD_In_d = motor_U.JD_In;
22677 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
22678
22679 /* Inport: '<Root>/YJ_In' */
22680 motor_B.YJ_In = motor_U.YJ_In;
22681
22682 /* Inport: '<Root>/Encode_Sp' */
22683 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
22684 motor_B.Slect_port_h = motor_U.System_Order;
22685
22686 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
22687 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
22688 motor_B.YJ_In, motor_B.Encode_Sp_l,
22689 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
22690 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
22691 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
22692 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
22693 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
22694 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
22695 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
22696 &motor_DWork.KG_clc, &motor_DWork.P_KP,
22697 &motor_DWork.V_KI, &motor_DWork.V_KP,
22698 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
22699 &motor_DWork.chu_jd);
22700
22701 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
22702 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
22703 if (tmp_1 < 65536.0) {
22704 if (tmp_1 >= 0.0) {
22705 tmp = (uint16_T)tmp_1;
22706 } else {
22707 tmp = 0U;
22708 }
22709 } else {
22710 tmp = MAX_uint16_T;
22711 }
22712
22713 motor_Y.PWMOUT = tmp;
22714 tmp_1 = motor_B.Motor_XHZY.KP_JD;
22715 if (tmp_1 < 2.147483648E+9) {
22716 if (tmp_1 >= -2.147483648E+9) {
22717 tmp_0 = (int32_T)tmp_1;
22718 } else {
22719 tmp_0 = MIN_int32_T;
22720 }
22721 } else {
22722 tmp_0 = MAX_int32_T;
22723 }
22724
22725 motor_Y.JD_Out = tmp_0;
22726 tmp_1 = motor_B.Motor_XHZY.KP_e;
22727 if (tmp_1 < 2.147483648E+9) {
22728 if (tmp_1 >= -2.147483648E+9) {
22729 tmp_0 = (int32_T)tmp_1;
22730 } else {
22731 tmp_0 = MIN_int32_T;
22732 }
22733 } else {
22734 tmp_0 = MAX_int32_T;
22735 }
22736
22737 motor_Y.JD_Error = tmp_0;
22738 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
22739 }
22740 break;
22741
22742 case motor_IN_Int1:
22743 /* Inport: '<Root>/Test_Mode' */
22744 /* During 'Int1': '<S1>:306' */
22745 if (motor_U.Test_Mode != 4) {
22746 /* Transition: '<S1>:737' */
22747 motor_DWork.is_Elevation_Test = motor_IN_Int4;
22748
22749 /* Entry 'Int4': '<S1>:308' */
22750 motor_DWork.KG_YJ = 0U;
22751
22752 /* Inport: '<Root>/JD_In' */
22753 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
22754 motor_B.JD_In_d = motor_U.JD_In;
22755 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
22756
22757 /* Inport: '<Root>/YJ_In' */
22758 motor_B.YJ_In = motor_U.YJ_In;
22759
22760 /* Inport: '<Root>/Encode_Sp' */
22761 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
22762 motor_B.Slect_port_h = motor_U.System_Order;
22763
22764 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
22765 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
22766 motor_B.YJ_In, motor_B.Encode_Sp_l,
22767 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
22768 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
22769 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
22770 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
22771 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
22772 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
22773 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
22774 &motor_DWork.KG_clc, &motor_DWork.P_KP,
22775 &motor_DWork.V_KI, &motor_DWork.V_KP,
22776 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
22777 &motor_DWork.chu_jd);
22778
22779 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
22780 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
22781 if (tmp_1 < 65536.0) {
22782 if (tmp_1 >= 0.0) {
22783 tmp = (uint16_T)tmp_1;
22784 } else {
22785 tmp = 0U;
22786 }
22787 } else {
22788 tmp = MAX_uint16_T;
22789 }
22790
22791 motor_Y.PWMOUT = tmp;
22792 tmp_1 = motor_B.Motor_XHZY.KP_JD;
22793 if (tmp_1 < 2.147483648E+9) {
22794 if (tmp_1 >= -2.147483648E+9) {
22795 tmp_0 = (int32_T)tmp_1;
22796 } else {
22797 tmp_0 = MIN_int32_T;
22798 }
22799 } else {
22800 tmp_0 = MAX_int32_T;
22801 }
22802
22803 motor_Y.JD_Out = tmp_0;
22804 tmp_1 = motor_B.Motor_XHZY.KP_e;
22805 if (tmp_1 < 2.147483648E+9) {
22806 if (tmp_1 >= -2.147483648E+9) {
22807 tmp_0 = (int32_T)tmp_1;
22808 } else {
22809 tmp_0 = MIN_int32_T;
22810 }
22811 } else {
22812 tmp_0 = MAX_int32_T;
22813 }
22814
22815 motor_Y.JD_Error = tmp_0;
22816 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
22817 } else {
22818 /* Inport: '<Root>/JD_In' */
22819 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
22820 motor_B.JD_In_d = motor_U.JD_In;
22821 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
22822
22823 /* Inport: '<Root>/YJ_In' */
22824 motor_B.YJ_In = motor_U.YJ_In;
22825
22826 /* Inport: '<Root>/Encode_Sp' */
22827 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
22828 motor_B.Slect_port_h = motor_U.System_Order;
22829
22830 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
22831 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
22832 motor_B.YJ_In, motor_B.Encode_Sp_l,
22833 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
22834 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
22835 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
22836 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
22837 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
22838 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
22839 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
22840 &motor_DWork.KG_clc, &motor_DWork.P_KP,
22841 &motor_DWork.V_KI, &motor_DWork.V_KP,
22842 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
22843 &motor_DWork.chu_jd);
22844
22845 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
22846 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
22847 if (tmp_1 < 65536.0) {
22848 if (tmp_1 >= 0.0) {
22849 tmp = (uint16_T)tmp_1;
22850 } else {
22851 tmp = 0U;
22852 }
22853 } else {
22854 tmp = MAX_uint16_T;
22855 }
22856
22857 motor_Y.PWMOUT = tmp;
22858 tmp_1 = motor_B.Motor_XHZY.KP_JD;
22859 if (tmp_1 < 2.147483648E+9) {
22860 if (tmp_1 >= -2.147483648E+9) {
22861 tmp_0 = (int32_T)tmp_1;
22862 } else {
22863 tmp_0 = MIN_int32_T;
22864 }
22865 } else {
22866 tmp_0 = MAX_int32_T;
22867 }
22868
22869 motor_Y.JD_Out = tmp_0;
22870 tmp_1 = motor_B.Motor_XHZY.KP_e;
22871 if (tmp_1 < 2.147483648E+9) {
22872 if (tmp_1 >= -2.147483648E+9) {
22873 tmp_0 = (int32_T)tmp_1;
22874 } else {
22875 tmp_0 = MIN_int32_T;
22876 }
22877 } else {
22878 tmp_0 = MAX_int32_T;
22879 }
22880
22881 motor_Y.JD_Error = tmp_0;
22882 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
22883 }
22884 break;
22885
22886 case motor_IN_Int3:
22887 /* During 'Int3': '<S1>:307' */
22888 /* Transition: '<S1>:733' */
22889 motor_DWork.is_Elevation_Test = motor_IN_NO_ACTIVE_CHILD;
22890 motor_DWork.is_Test_Mode = motor_IN_defult;
22891
22892 /* Entry 'defult': '<S1>:239' */
22893 motor_Y.Open_Result = 1U;
22894
22895 /* 开机状态位成功 */
22896 motor_DWork.In_State = 2U;
22897 break;
22898
22899 default:
22900 /* During 'Int4': '<S1>:308' */
22901 if (motor_Y.JD_Error < 0.1) {
22902 /* Transition: '<S1>:735' */
22903 motor_DWork.is_Elevation_Test = motor_IN_Int3;
22904
22905 /* Entry 'Int3': '<S1>:307' */
22906 motor_Y.Motor_En = true;
22907
22908 /* 电机失能 */
22909 motor_Y.PWMOUT = motor_DWork.PWM_Value_Mid;
22910 } else {
22911 /* Inport: '<Root>/JD_In' */
22912 /* Simulink Function 'Motor_XHZY': '<S1>:90' */
22913 motor_B.JD_In_d = motor_U.JD_In;
22914 motor_B.Encode_Pos_f = motor_Y.Encode_Pos;
22915
22916 /* Inport: '<Root>/YJ_In' */
22917 motor_B.YJ_In = motor_U.YJ_In;
22918
22919 /* Inport: '<Root>/Encode_Sp' */
22920 motor_B.Encode_Sp_l = motor_U.Encode_Sp;
22921 motor_B.Slect_port_h = motor_U.System_Order;
22922
22923 /* Outputs for Function Call SubSystem: '<S1>/Motor_XHZY' */
22924 motor_Motor_XHZY(motor_M, motor_B.JD_In_d, motor_B.Encode_Pos_f,
22925 motor_B.YJ_In, motor_B.Encode_Sp_l,
22926 motor_B.Slect_port_h, &motor_B.Motor_XHZY,
22927 &motor_DWork.Motor_XHZY, (rtP_Motor_XHZY_motor *)
22928 &motor_P.Motor_XHZY, &motor_DWork.Angle_extern,
22929 &motor_DWork.Angle_internal, &motor_DWork.EN_extern,
22930 &motor_DWork.Forword, &motor_DWork.JD_XHZY,
22931 &motor_DWork.JD_XHZY_HC, &motor_DWork.KG_En,
22932 &motor_DWork.KG_JD, &motor_DWork.KG_YJ,
22933 &motor_DWork.KG_clc, &motor_DWork.P_KP,
22934 &motor_DWork.V_KI, &motor_DWork.V_KP,
22935 &motor_DWork.YJ_XHZY, &motor_DWork.YJ_XHZY_HC,
22936 &motor_DWork.chu_jd);
22937
22938 /* End of Outputs for SubSystem: '<S1>/Motor_XHZY' */
22939 tmp_1 = motor_B.Motor_XHZY.ZXF_PWM;
22940 if (tmp_1 < 65536.0) {
22941 if (tmp_1 >= 0.0) {
22942 tmp = (uint16_T)tmp_1;
22943 } else {
22944 tmp = 0U;
22945 }
22946 } else {
22947 tmp = MAX_uint16_T;
22948 }
22949
22950 motor_Y.PWMOUT = tmp;
22951 tmp_1 = motor_B.Motor_XHZY.KP_JD;
22952 if (tmp_1 < 2.147483648E+9) {
22953 if (tmp_1 >= -2.147483648E+9) {
22954 tmp_0 = (int32_T)tmp_1;
22955 } else {
22956 tmp_0 = MIN_int32_T;
22957 }
22958 } else {
22959 tmp_0 = MAX_int32_T;
22960 }
22961
22962 motor_Y.JD_Out = tmp_0;
22963 tmp_1 = motor_B.Motor_XHZY.KP_e;
22964 if (tmp_1 < 2.147483648E+9) {
22965 if (tmp_1 >= -2.147483648E+9) {
22966 tmp_0 = (int32_T)tmp_1;
22967 } else {
22968 tmp_0 = MIN_int32_T;
22969 }
22970 } else {
22971 tmp_0 = MAX_int32_T;
22972 }
22973
22974 motor_Y.JD_Error = tmp_0;
22975 motor_Y.SGWY_Out = motor_B.Motor_XHZY.DataTypeConversion4;
22976 }
22977 break;
22978 }
22979 break;
22980
22981 case motor_IN_Limit_Down_Test:
22982 motor_Limit_Down_Test();
22983 break;
22984
22985 case motor_IN_Limit_Up_Test:
22986 motor_Limit_Up_Test();
22987 break;
22988
22989 default:
22990 motor_defult();
22991 break;
22992 }
22993 }
22994 break;
22995 }
22996
22997 if (guard1) {
22998 /* Inport: '<Root>/System_Order' */
22999 if (motor_U.System_Order == 1) {
23000 /* Transition: '<S1>:388' */
23001 /* Exit Internal 'Close': '<S1>:130' */
23002 /* Exit Internal 'Algorithm': '<S1>:131' */
23003 /* Exit Internal 'HYDG_Close': '<S1>:132' */
23004 motor_DWork.is_HYDG_Close = motor_IN_NO_ACTIVE_CHILD;
23005 motor_DWork.is_Algorithm = motor_IN_NO_ACTIVE_CHILD;
23006
23007 /* Exit Internal 'XHHY_Close': '<S1>:136' */
23008 motor_DWork.is_XHHY_Close = motor_IN_NO_ACTIVE_CHILD;
23009
23010 /* Exit Internal 'XHZY_Close': '<S1>:140' */
23011 motor_DWork.is_XHZY_Close = motor_IN_NO_ACTIVE_CHILD;
23012 motor_DWork.is_Close = motor_IN_NO_ACTIVE_CHILD;
23013 motor_DWork.is_M_Run = motor_IN_Initialize;
23014
23015 /* Entry 'Initialize': '<S1>:12' */
23016 motor_Y.Ini_Result = 2U;
23017
23018 /* 初始化未完成 */
23019 motor_DWork.In_State = 1U;
23020
23021 /* Entry Internal 'Initialize': '<S1>:12' */
23022 /* Transition: '<S1>:396' */
23023 motor_DWork.is_Initialize = motor_IN_Parameters_Reset0;
23024 motor_DWork.temporalCounter_i1 = 0U;
23025
23026 /* Entry 'Parameters_Reset0': '<S1>:89' */
23027 motor_Y.DCZD = false;
23028
23029 /* 解除制动 */
23030 } else if (motor_DWork.is_Close == motor_IN_Algorithm) {
23031 motor_Algorithm();
23032 } else {
23033 /* During 'Close_Wait': '<S1>:241' */
23034 if (motor_DWork.temporalCounter_i1 >= (uint32_T)(0.5 / motor_DWork.Ts)) {
23035 /* Inport: '<Root>/Motor_Num' */
23036 /* Transition: '<S1>:497' */
23037 if (motor_U.Motor_Num == 1) {
23038 /* Transition: '<S1>:502' */
23039 motor_DWork.is_Close = motor_IN_Algorithm;
23040 motor_DWork.is_Algorithm = motor_IN_XHZY_Close;
23041
23042 /* Entry 'XHZY_Close': '<S1>:140' */
23043 motor_DWork.KG_En = 1U;
23044 motor_DWork.KG_JD = 0U;
23045 motor_DWork.KG_YJ = 0U;
23046 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
23047 motor_DWork.chu_jd = 0.0;
23048
23049 /* Entry Internal 'XHZY_Close': '<S1>:140' */
23050 /* Transition: '<S1>:523' */
23051 motor_DWork.is_XHZY_Close = motor_IN_Defualt;
23052 motor_DWork.temporalCounter_i1 = 0U;
23053 } else if (motor_U.Motor_Num == 2) {
23054 /* Transition: '<S1>:503' */
23055 motor_DWork.is_Close = motor_IN_Algorithm;
23056 motor_DWork.is_Algorithm = motor_IN_HYDG_Close;
23057
23058 /* Entry 'HYDG_Close': '<S1>:132' */
23059 motor_DWork.KG_JD = 0U;
23060 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
23061
23062 /* Entry Internal 'HYDG_Close': '<S1>:132' */
23063 /* Transition: '<S1>:506' */
23064 motor_DWork.is_HYDG_Close = motor_IN_Int_k;
23065
23066 /* Entry 'Int': '<S1>:312' */
23067 /* Simulink Function 'Angle_Calculation_HY': '<S1>:253' */
23068 motor_B.Encode_Pos_dc = motor_Y.Encode_Pos;
23069
23070 /* Outputs for Function Call SubSystem: '<S1>/Angle_Calculation_HY' */
23071 motor_Angle_Calculation_HY(motor_B.Encode_Pos_dc,
23072 &motor_B.Angle_Calculation_HY, (rtP_Angle_Calculation_HY_motor *)
23073 &motor_P.Angle_Calculation_HY);
23074
23075 /* End of Outputs for SubSystem: '<S1>/Angle_Calculation_HY' */
23076 motor_DWork.chu_jd = motor_B.Angle_Calculation_HY.GHDG9;
23077 motor_DWork.KG = 1U;
23078 motor_DWork.KG_JD = 0U;
23079 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
23080 motor_DWork.Saturation_limit_speed = 16000.0;
23081 motor_DWork.Rate_limit_speed = 50000.0;
23082 } else {
23083 if (motor_U.Motor_Num == 3) {
23084 /* Transition: '<S1>:504' */
23085 motor_DWork.is_Close = motor_IN_Algorithm;
23086 motor_DWork.is_Algorithm = motor_IN_XHHY_Close;
23087
23088 /* Entry 'XHHY_Close': '<S1>:136' */
23089 motor_DWork.KG_En = 1U;
23090 motor_DWork.KG_JD = 0U;
23091 motor_DWork.KG_clc = (uint8_T)!(motor_DWork.KG_clc != 0);
23092 motor_DWork.chu_jd = 0.0;
23093
23094 /* Entry Internal 'XHHY_Close': '<S1>:136' */
23095 /* Transition: '<S1>:516' */
23096 motor_DWork.is_XHHY_Close = motor_IN_Defualt;
23097 motor_DWork.temporalCounter_i1 = 0U;
23098 }
23099 }
23100
23101 /* End of Inport: '<Root>/Motor_Num' */
23102 }
23103 }
23104 }
23105}
23106
23107/* Function for Chart: '<Root>/Chart' */
23108static void motor_Ready_Run(void)
23109{
23110 uint32_T q0;
23111 uint32_T qY;
23112 int32_T saturatedUnaryMinus;
23113 uint16_T tmp;
23114 real_T tmp_0;
23115
23116 /* During 'Ready_Run': '<S1>:2' */
23117 /* During 'Current_check': '<S1>:3' */
23118 switch (motor_DWork.is_Current_check) {
23119 case motor_IN_p1:
23120 /* Inport: '<Root>/Cur_In' */
23121 /* During 'p1': '<S1>:4' */
23122 if ((motor_U.Cur_In >= motor_DWork.Cur_Limit) || (motor_U.Cur_In <=
23123 motor_DWork.Cur_Limit_Down)) {
23124 /* Transition: '<S1>:371' */
23125 motor_DWork.is_Current_check = motor_IN_p2;
23126
23127 /* Entry 'p2': '<S1>:7' */
23128 motor_DWork.cur_i = 1U;
23129 }
23130 break;
23131
23132 case motor_IN_p2:
23133 /* Inport: '<Root>/Cur_In' */
23134 /* During 'p2': '<S1>:7' */
23135 if ((motor_U.Cur_In < motor_DWork.Cur_Limit) && (motor_U.Cur_In >
23136 motor_DWork.Cur_Limit_Down)) {
23137 /* Transition: '<S1>:372' */
23138 motor_DWork.is_Current_check = motor_IN_p1;
23139
23140 /* Entry 'p1': '<S1>:4' */
23141 motor_DWork.cur_i = 0U;
23142
23143 /* 计数器置零 */
23144 motor_Y.Flag_Cur = 1U;
23145
23146 /* 电流正常 */
23147 } else {
23148 tmp_0 = 2.0 / motor_DWork.Ts;
23149 if (tmp_0 < 65536.0) {
23150 if (tmp_0 >= 0.0) {
23151 tmp = (uint16_T)tmp_0;
23152 } else {
23153 tmp = 0U;
23154 }
23155 } else {
23156 tmp = MAX_uint16_T;
23157 }
23158
23159 if (motor_DWork.cur_i >= tmp) {
23160 /* Transition: '<S1>:376' */
23161 /* 2S */
23162 motor_DWork.is_Current_check = motor_IN_p3;
23163
23164 /* Entry 'p3': '<S1>:6' */
23165 /* 0代表正常,1代表错误 */
23166 motor_DWork.cur_i = 0U;
23167 motor_Y.Flag_Cur = 0U;
23168
23169 /* 电机过流 */
23170 motor_Y.Flag_GZ_Cur = 1U;
23171
23172 /* 电机未严重过流 */
23173 } else {
23174 saturatedUnaryMinus = motor_DWork.cur_i + 1;
23175 if (saturatedUnaryMinus > 65535) {
23176 saturatedUnaryMinus = 65535;
23177 }
23178
23179 motor_DWork.cur_i = (uint16_T)saturatedUnaryMinus;
23180 }
23181 }
23182 break;
23183
23184 case motor_IN_p3:
23185 /* Inport: '<Root>/Cur_In' */
23186 /* During 'p3': '<S1>:6' */
23187 if ((motor_U.Cur_In < motor_DWork.Cur_Limit) && (motor_U.Cur_In >
23188 motor_DWork.Cur_Limit_Down)) {
23189 /* Transition: '<S1>:374' */
23190 motor_DWork.is_Current_check = motor_IN_p4;
23191
23192 /* Entry 'p4': '<S1>:5' */
23193 motor_DWork.cur_i = 1U;
23194 } else {
23195 if ((motor_U.Cur_In >= motor_DWork.Cur_GZ_Limit) || (motor_U.Cur_In <=
23196 motor_DWork.Cur_GZ_Limit_Down)) {
23197 /* Transition: '<S1>:377' */
23198 motor_DWork.is_Current_check = motor_IN_p5;
23199
23200 /* Entry 'p5': '<S1>:8' */
23201 motor_DWork.cur_i = 1U;
23202 }
23203 }
23204 break;
23205
23206 case motor_IN_p4:
23207 /* Inport: '<Root>/Cur_In' */
23208 /* During 'p4': '<S1>:5' */
23209 if ((motor_U.Cur_In >= motor_DWork.Cur_Limit) || (motor_U.Cur_In <=
23210 motor_DWork.Cur_Limit_Down)) {
23211 /* Transition: '<S1>:373' */
23212 motor_DWork.is_Current_check = motor_IN_p3;
23213
23214 /* Entry 'p3': '<S1>:6' */
23215 /* 0代表正常,1代表错误 */
23216 motor_DWork.cur_i = 0U;
23217 motor_Y.Flag_Cur = 0U;
23218
23219 /* 电机过流 */
23220 motor_Y.Flag_GZ_Cur = 1U;
23221
23222 /* 电机未严重过流 */
23223 } else {
23224 tmp_0 = 2.0 / motor_DWork.Ts;
23225 if (tmp_0 < 65536.0) {
23226 if (tmp_0 >= 0.0) {
23227 tmp = (uint16_T)tmp_0;
23228 } else {
23229 tmp = 0U;
23230 }
23231 } else {
23232 tmp = MAX_uint16_T;
23233 }
23234
23235 if (motor_DWork.cur_i >= tmp) {
23236 /* Transition: '<S1>:370' */
23237 motor_DWork.is_Current_check = motor_IN_p1;
23238
23239 /* Entry 'p1': '<S1>:4' */
23240 motor_DWork.cur_i = 0U;
23241
23242 /* 计数器置零 */
23243 motor_Y.Flag_Cur = 1U;
23244
23245 /* 电流正常 */
23246 } else {
23247 saturatedUnaryMinus = motor_DWork.cur_i + 1;
23248 if (saturatedUnaryMinus > 65535) {
23249 saturatedUnaryMinus = 65535;
23250 }
23251
23252 motor_DWork.cur_i = (uint16_T)saturatedUnaryMinus;
23253 }
23254 }
23255 break;
23256
23257 case motor_IN_p5:
23258 /* During 'p5': '<S1>:8' */
23259 tmp_0 = 2.0 / motor_DWork.Ts;
23260 if (tmp_0 < 65536.0) {
23261 if (tmp_0 >= 0.0) {
23262 tmp = (uint16_T)tmp_0;
23263 } else {
23264 tmp = 0U;
23265 }
23266 } else {
23267 tmp = MAX_uint16_T;
23268 }
23269
23270 if (motor_DWork.cur_i >= tmp) {
23271 /* Transition: '<S1>:379' */
23272 /* 2S */
23273 motor_DWork.is_Current_check = motor_IN_p6;
23274
23275 /* Entry 'p6': '<S1>:10' */
23276 /* Motor_En=1;%电机失能 */
23277 motor_Y.Flag_GZ_Cur = 0U;
23278
23279 /* 电机严重过流 */
23280 } else if ((motor_U.Cur_In < motor_DWork.Cur_GZ_Limit) && (motor_U.Cur_In >
23281 motor_DWork.Cur_GZ_Limit_Down)) {
23282 /* Transition: '<S1>:378' */
23283 motor_DWork.is_Current_check = motor_IN_p3;
23284
23285 /* Entry 'p3': '<S1>:6' */
23286 /* 0代表正常,1代表错误 */
23287 motor_DWork.cur_i = 0U;
23288 motor_Y.Flag_Cur = 0U;
23289
23290 /* 电机过流 */
23291 motor_Y.Flag_GZ_Cur = 1U;
23292
23293 /* 电机未严重过流 */
23294 } else {
23295 saturatedUnaryMinus = motor_DWork.cur_i + 1;
23296 if (saturatedUnaryMinus > 65535) {
23297 saturatedUnaryMinus = 65535;
23298 }
23299
23300 motor_DWork.cur_i = (uint16_T)saturatedUnaryMinus;
23301 }
23302 break;
23303
23304 case motor_IN_p6:
23305 /* Inport: '<Root>/Cur_In' */
23306 /* During 'p6': '<S1>:10' */
23307 if ((motor_U.Cur_In < motor_DWork.Cur_GZ_Limit) && (motor_U.Cur_In >
23308 motor_DWork.Cur_GZ_Limit_Down)) {
23309 /* Transition: '<S1>:380' */
23310 motor_DWork.is_Current_check = motor_IN_p7;
23311
23312 /* Entry 'p7': '<S1>:9' */
23313 motor_DWork.cur_i = 1U;
23314 }
23315 break;
23316
23317 default:
23318 /* Inport: '<Root>/Cur_In' */
23319 /* During 'p7': '<S1>:9' */
23320 if ((motor_U.Cur_In >= motor_DWork.Cur_GZ_Limit) || (motor_U.Cur_In <=
23321 motor_DWork.Cur_GZ_Limit_Down)) {
23322 /* Transition: '<S1>:381' */
23323 motor_DWork.is_Current_check = motor_IN_p6;
23324
23325 /* Entry 'p6': '<S1>:10' */
23326 /* Motor_En=1;%电机失能 */
23327 motor_Y.Flag_GZ_Cur = 0U;
23328
23329 /* 电机严重过流 */
23330 } else {
23331 tmp_0 = 2.0 / motor_DWork.Ts;
23332 if (tmp_0 < 65536.0) {
23333 if (tmp_0 >= 0.0) {
23334 tmp = (uint16_T)tmp_0;
23335 } else {
23336 tmp = 0U;
23337 }
23338 } else {
23339 tmp = MAX_uint16_T;
23340 }
23341
23342 if (motor_DWork.cur_i >= tmp) {
23343 /* Transition: '<S1>:375' */
23344 motor_DWork.is_Current_check = motor_IN_p3;
23345
23346 /* Entry 'p3': '<S1>:6' */
23347 /* 0代表正常,1代表错误 */
23348 motor_DWork.cur_i = 0U;
23349 motor_Y.Flag_Cur = 0U;
23350
23351 /* 电机过流 */
23352 motor_Y.Flag_GZ_Cur = 1U;
23353
23354 /* 电机未严重过流 */
23355 } else {
23356 saturatedUnaryMinus = motor_DWork.cur_i + 1;
23357 if (saturatedUnaryMinus > 65535) {
23358 saturatedUnaryMinus = 65535;
23359 }
23360
23361 motor_DWork.cur_i = (uint16_T)saturatedUnaryMinus;
23362 }
23363 }
23364 break;
23365 }
23366
23367 /* Inport: '<Root>/Temp_In' incorporates:
23368 * Inport: '<Root>/Cur_In'
23369 */
23370 /* During 'Temp_check': '<S1>:56' */
23371 q0 = motor_U.Temp_In;
23372 if (q0 > 65535U) {
23373 q0 = 65535U;
23374 }
23375
23376 qY = motor_DWork.Temp_Up_Limit;
23377 if (qY > 65535U) {
23378 qY = 65535U;
23379 }
23380
23381 motor_Y.Flag_Temp_Up = motor_Temp_Up_Check((uint16_T)q0, (uint16_T)qY);
23382
23383 /* Inport: '<Root>/Temp_In' */
23384 q0 = motor_U.Temp_In;
23385 if (q0 > 65535U) {
23386 q0 = 65535U;
23387 }
23388
23389 qY = motor_DWork.Temp_Down_Limit;
23390 if (qY > 65535U) {
23391 qY = 65535U;
23392 }
23393
23394 motor_Y.Flag_Temp_Down = motor_Temp_Down_Check((uint16_T)q0, (uint16_T)qY);
23395 motor_M_Run();
23396
23397 /* During 'ZHI_S_D': '<S1>:60' */
23398 /* 指示灯 */
23399 /* Simulink Function 'BJ': '<S1>:100' */
23400 motor_B.x = motor_DWork.In_State;
23401 motor_B.y = motor_Y.QDQ_BJ;
23402
23403 /* Outputs for Function Call SubSystem: '<S1>/BJ' */
23404 /* DataStoreRead: '<S5>/Data Store Read1' */
23405 motor_B.DataStoreRead1 = motor_DWork.PC1;
23406
23407 /* DataStoreRead: '<S5>/Data Store Read' */
23408 motor_B.DataStoreRead = motor_DWork.PC2;
23409
23410 /* SwitchCase: '<S5>/Switch Case' */
23411 switch (motor_B.x) {
23412 case 1:
23413 /* Outputs for IfAction SubSystem: '<S5>/If Action Subsystem' incorporates:
23414 * ActionPort: '<S23>/Action Port'
23415 */
23416 motor_IfActionSubsystem_n(motor_B.y, &motor_B.IfActionSubsystem,
23417 &motor_DWork.IfActionSubsystem, (rtP_IfActionSubsystem_motor *)
23418 &motor_P.IfActionSubsystem, &motor_DWork.PC1, &motor_DWork.PC2);
23419
23420 /* End of Outputs for SubSystem: '<S5>/If Action Subsystem' */
23421 break;
23422
23423 case 2:
23424 /* Outputs for IfAction SubSystem: '<S5>/If Action Subsystem1' incorporates:
23425 * ActionPort: '<S24>/Action Port'
23426 */
23427 /* If: '<S24>/If' */
23428 if (motor_B.y == 2.0) {
23429 /* Outputs for IfAction SubSystem: '<S24>/If Action Subsystem' incorporates:
23430 * ActionPort: '<S29>/Action Port'
23431 */
23432 motor_IfActionSubsystem(&motor_B.IfActionSubsystem_f,
23433 &motor_DWork.IfActionSubsystem_f, (rtP_IfActionSubsystem_motor_m *)
23434 &motor_P.IfActionSubsystem_f, &motor_DWork.PC1, &motor_DWork.PC2);
23435
23436 /* End of Outputs for SubSystem: '<S24>/If Action Subsystem' */
23437 } else if (motor_B.y == 3.0) {
23438 /* Outputs for IfAction SubSystem: '<S24>/If Action Subsystem1' incorporates:
23439 * ActionPort: '<S30>/Action Port'
23440 */
23441 motor_IfActionSubsystem1(&motor_B.IfActionSubsystem1_m,
23442 &motor_DWork.IfActionSubsystem1_m, (rtP_IfActionSubsystem1_motor *)
23443 &motor_P.IfActionSubsystem1_m, &motor_DWork.PC1, &motor_DWork.PC2);
23444
23445 /* End of Outputs for SubSystem: '<S24>/If Action Subsystem1' */
23446 } else {
23447 /* Outputs for IfAction SubSystem: '<S24>/If Action Subsystem2' incorporates:
23448 * ActionPort: '<S31>/Action Port'
23449 */
23450 /* DataStoreWrite: '<S31>/Data Store Write1' incorporates:
23451 * Constant: '<S31>/Constant'
23452 */
23453 motor_DWork.PC1 = motor_P.Constant_Value;
23454
23455 /* DiscretePulseGenerator: '<S31>/Pulse Generator' */
23456 motor_B.PulseGenerator = (motor_DWork.clockTickCounter <
23457 motor_P.PulseGenerator_Duty) && (motor_DWork.clockTickCounter >= 0) ?
23458 motor_P.PulseGenerator_Amp : 0.0;
23459 if (motor_DWork.clockTickCounter >= motor_P.PulseGenerator_Period - 1.0) {
23460 motor_DWork.clockTickCounter = 0;
23461 } else {
23462 motor_DWork.clockTickCounter++;
23463 }
23464
23465 /* End of DiscretePulseGenerator: '<S31>/Pulse Generator' */
23466
23467 /* DataTypeConversion: '<S31>/Data Type Conversion' */
23468 motor_B.DataTypeConversion = (motor_B.PulseGenerator != 0.0);
23469
23470 /* DataStoreWrite: '<S31>/Data Store Write' */
23471 motor_DWork.PC2 = motor_B.DataTypeConversion;
23472
23473 /* End of Outputs for SubSystem: '<S24>/If Action Subsystem2' */
23474 }
23475
23476 /* End of If: '<S24>/If' */
23477 /* End of Outputs for SubSystem: '<S5>/If Action Subsystem1' */
23478 break;
23479
23480 case 3:
23481 /* Outputs for IfAction SubSystem: '<S5>/If Action Subsystem2' incorporates:
23482 * ActionPort: '<S25>/Action Port'
23483 */
23484 motor_IfActionSubsystem_n(motor_B.y, &motor_B.IfActionSubsystem2,
23485 &motor_DWork.IfActionSubsystem2, (rtP_IfActionSubsystem_motor *)
23486 &motor_P.IfActionSubsystem2, &motor_DWork.PC1, &motor_DWork.PC2);
23487
23488 /* End of Outputs for SubSystem: '<S5>/If Action Subsystem2' */
23489 break;
23490 }
23491
23492 /* End of SwitchCase: '<S5>/Switch Case' */
23493 /* End of Outputs for SubSystem: '<S1>/BJ' */
23494 motor_Y.QDQ_HY = motor_B.DataStoreRead1;
23495 motor_Y.QDQ_XH = motor_B.DataStoreRead;
23496 switch (motor_DWork.is_ZHI_S_D) {
23497 case motor_IN_QDQ_BJ:
23498 /* During 'QDQ_BJ': '<S1>:123' */
23499 if ((motor_Y.Flag_Up_GZ_limit == 0) || (motor_Y.Flag_Down_GZ_limit == 0) ||
23500 (motor_Y.Flag_GZ_Cur == 0) || (motor_Y.Flag_Motor_Error == 0) ||
23501 (motor_Y.Flag_Temp_Down == 0) || (motor_Y.Flag_Enc_Error == 0) ||
23502 (motor_Y.Flag_GXDY_IT == 0)) {
23503 /* Transition: '<S1>:450' */
23504 motor_DWork.is_ZHI_S_D = motor_IN_QDQ_GZ;
23505
23506 /* Entry 'QDQ_GZ': '<S1>:122' */
23507 motor_Y.QDQ_BJ = 3U;
23508 motor_Y.Alarm_Cunt = 3U;
23509 motor_Y.Motor_En = true;
23510 } else if ((motor_Y.Flag_Cur == 1) && (motor_Y.Flag_Temp_Up == 1) &&
23511 (motor_Y.Flag_Up_limit == 1) && (motor_Y.Flag_Down_limit == 1) &&
23512 (motor_Y.Flag_GXDY_Error == 1) && (motor_Y.Flag_AngleError == 0))
23513 {
23514 /* Transition: '<S1>:448' */
23515 motor_DWork.is_ZHI_S_D = motor_IN_defult_b;
23516
23517 /* Entry 'defult': '<S1>:70' */
23518 motor_Y.QDQ_BJ = 1U;
23519 motor_Y.Alarm_Cunt = 1U;
23520 } else {
23521 motor_Y.Alarm_Cunt = 1U;
23522 }
23523 break;
23524
23525 case motor_IN_QDQ_GZ:
23526 /* During 'QDQ_GZ': '<S1>:122' */
23527 if ((motor_Y.Flag_GZ_Cur == 1) && (motor_Y.Flag_Motor_Error == 1) &&
23528 (motor_Y.Flag_Temp_Down == 1) && (motor_Y.Flag_Enc_Error == 1) &&
23529 (motor_Y.Flag_GXDY_IT == 1)) {
23530 /* Transition: '<S1>:1137' */
23531 motor_DWork.is_ZHI_S_D = motor_IN_defult_b;
23532
23533 /* Entry 'defult': '<S1>:70' */
23534 motor_Y.QDQ_BJ = 1U;
23535 motor_Y.Alarm_Cunt = 1U;
23536 } else {
23537 motor_Y.Alarm_Cunt = 1U;
23538 }
23539 break;
23540
23541 default:
23542 /* During 'defult': '<S1>:70' */
23543 if ((motor_Y.Flag_Cur == 0) || (motor_Y.Flag_Temp_Up == 0) ||
23544 (motor_Y.Flag_Up_limit == 0) || (motor_Y.Flag_Down_limit == 0) ||
23545 (motor_Y.Flag_GXDY_Error == 0) || (motor_Y.Flag_AngleError == 0)) {
23546 /* Transition: '<S1>:447' */
23547 motor_DWork.is_ZHI_S_D = motor_IN_QDQ_BJ;
23548
23549 /* Entry 'QDQ_BJ': '<S1>:123' */
23550 motor_Y.QDQ_BJ = 2U;
23551 motor_Y.Alarm_Cunt = 2U;
23552 } else {
23553 if ((motor_Y.Flag_GZ_Cur == 0) || (motor_Y.Flag_Motor_Error == 0) ||
23554 (motor_Y.Flag_Temp_Down == 0) || (motor_Y.Flag_Enc_Error == 0) ||
23555 (motor_Y.Flag_GXDY_IT == 0)) {
23556 /* Transition: '<S1>:449' */
23557 motor_DWork.is_ZHI_S_D = motor_IN_QDQ_GZ;
23558
23559 /* Entry 'QDQ_GZ': '<S1>:122' */
23560 motor_Y.QDQ_BJ = 3U;
23561 motor_Y.Alarm_Cunt = 3U;
23562 motor_Y.Motor_En = true;
23563 }
23564 }
23565 break;
23566 }
23567
23568 /* During 'Fault_Exit': '<S1>:1100' */
23569 /* During 'Limit_Fault_Exit': '<S1>:1128' */
23570 switch (motor_DWork.is_Limit_Fault_Exit) {
23571 case motor_IN_defult_bq:
23572 /* During 'defult': '<S1>:1107' */
23573 saturatedUnaryMinus = motor_Y.JD_Out;
23574 if (saturatedUnaryMinus < 0) {
23575 if (saturatedUnaryMinus <= MIN_int32_T) {
23576 saturatedUnaryMinus = MAX_int32_T;
23577 } else {
23578 saturatedUnaryMinus = -saturatedUnaryMinus;
23579 }
23580 }
23581
23582 /* Inport: '<Root>/Up_Limit' */
23583 if ((motor_U.Up_Limit == 0) && (saturatedUnaryMinus > 10000)) {
23584 /* Transition: '<S1>:1130' */
23585 motor_DWork.is_Limit_Fault_Exit = motor_IN_defult1_k;
23586 motor_DWork.temporalCounter_i7 = 0U;
23587
23588 /* Entry 'defult1': '<S1>:1129' */
23589 motor_Y.Flag_Up_GZ_limit = 1U;
23590 } else {
23591 saturatedUnaryMinus = motor_Y.JD_Out;
23592 if (saturatedUnaryMinus < 0) {
23593 if (saturatedUnaryMinus <= MIN_int32_T) {
23594 saturatedUnaryMinus = MAX_int32_T;
23595 } else {
23596 saturatedUnaryMinus = -saturatedUnaryMinus;
23597 }
23598 }
23599
23600 /* Inport: '<Root>/Down_Limit' */
23601 if ((motor_U.Down_Limit == 0) && (saturatedUnaryMinus > 10000)) {
23602 /* Transition: '<S1>:1133' */
23603 motor_DWork.is_Limit_Fault_Exit = motor_IN_defult2_e;
23604 motor_DWork.temporalCounter_i7 = 0U;
23605
23606 /* Entry 'defult2': '<S1>:1132' */
23607 motor_Y.Flag_Down_GZ_limit = 1U;
23608 }
23609
23610 /* End of Inport: '<Root>/Down_Limit' */
23611 }
23612
23613 /* End of Inport: '<Root>/Up_Limit' */
23614 break;
23615
23616 case motor_IN_defult1_k:
23617 /* During 'defult1': '<S1>:1129' */
23618 if (motor_DWork.temporalCounter_i7 >= (uint32_T)(10.0 / motor_DWork.Ts)) {
23619 /* Transition: '<S1>:1131' */
23620 motor_DWork.is_Limit_Fault_Exit = motor_IN_defult_bq;
23621 }
23622 break;
23623
23624 default:
23625 /* During 'defult2': '<S1>:1132' */
23626 if (motor_DWork.temporalCounter_i7 >= (uint32_T)(10.0 / motor_DWork.Ts)) {
23627 /* Transition: '<S1>:1134' */
23628 motor_DWork.is_Limit_Fault_Exit = motor_IN_defult_bq;
23629 }
23630 break;
23631 }
23632
23633 /* During 'Motor_check': '<S1>:1144' */
23634 switch (motor_DWork.is_Motor_check) {
23635 case motor_IN_defult_bq:
23636 /* Inport: '<Root>/Encode_Sp' */
23637 /* During 'defult': '<S1>:1151' */
23638 if (motor_U.Encode_Sp < 0) {
23639 saturatedUnaryMinus = motor_U.Encode_Sp;
23640 if (saturatedUnaryMinus <= MIN_int32_T) {
23641 saturatedUnaryMinus = MAX_int32_T;
23642 } else {
23643 saturatedUnaryMinus = -saturatedUnaryMinus;
23644 }
23645 } else {
23646 saturatedUnaryMinus = motor_U.Encode_Sp;
23647 }
23648
23649 /* End of Inport: '<Root>/Encode_Sp' */
23650
23651 /* Inport: '<Root>/Cur_In' */
23652 q0 = motor_U.Cur_In;
23653 qY = q0 - 1925U;
23654 if (qY > q0) {
23655 qY = 0U;
23656 }
23657
23658 if (((motor_Y.PWMOUT > 4000) || (motor_Y.PWMOUT < 1000)) &&
23659 (!motor_Y.Motor_En) && (qY < 10U) && (saturatedUnaryMinus < 3)) {
23660 /* Transition: '<S1>:1155' */
23661 motor_DWork.is_Motor_check = motor_IN_defult1_k;
23662 motor_DWork.temporalCounter_i8 = 0U;
23663 } else {
23664 saturatedUnaryMinus = motor_Y.JD_Error;
23665 if (saturatedUnaryMinus < 0) {
23666 if (saturatedUnaryMinus <= MIN_int32_T) {
23667 saturatedUnaryMinus = MAX_int32_T;
23668 } else {
23669 saturatedUnaryMinus = -saturatedUnaryMinus;
23670 }
23671 }
23672
23673 if ((!motor_Y.Motor_En) && (motor_DWork.Runing_stable == 1) &&
23674 (saturatedUnaryMinus > 1500)) {
23675 /* Transition: '<S1>:1292' */
23676 motor_DWork.is_Motor_check = motor_IN_defult1_k;
23677 motor_DWork.temporalCounter_i8 = 0U;
23678 }
23679 }
23680 break;
23681
23682 case motor_IN_defult1_k:
23683 /* During 'defult1': '<S1>:1154' */
23684 if (motor_DWork.temporalCounter_i8 >= (uint32_T)(2.0 / motor_DWork.Ts)) {
23685 /* Transition: '<S1>:1157' */
23686 motor_DWork.is_Motor_check = motor_IN_defult2_e;
23687 motor_DWork.temporalCounter_i8 = 0U;
23688
23689 /* Entry 'defult2': '<S1>:1156' */
23690 motor_Y.Flag_Motor_Error = 0U;
23691 } else {
23692 saturatedUnaryMinus = motor_Y.JD_Error;
23693 if (saturatedUnaryMinus < 0) {
23694 if (saturatedUnaryMinus <= MIN_int32_T) {
23695 saturatedUnaryMinus = MAX_int32_T;
23696 } else {
23697 saturatedUnaryMinus = -saturatedUnaryMinus;
23698 }
23699 }
23700
23701 if (motor_Y.Motor_En || (motor_DWork.Runing_stable == 0) ||
23702 (saturatedUnaryMinus <= 1500)) {
23703 /* Transition: '<S1>:1293' */
23704 motor_DWork.is_Motor_check = motor_IN_defult_bq;
23705
23706 /* Entry 'defult': '<S1>:1151' */
23707 motor_Y.Flag_Motor_Error = 1U;
23708 }
23709 }
23710 break;
23711
23712 case motor_IN_defult2_e:
23713 /* During 'defult2': '<S1>:1156' */
23714 if (motor_DWork.temporalCounter_i8 >= (uint32_T)(5.0 / motor_DWork.Ts)) {
23715 /* Transition: '<S1>:1160' */
23716 motor_DWork.is_Motor_check = motor_IN_defult3;
23717 }
23718 break;
23719
23720 case motor_IN_defult3:
23721 /* During 'defult3': '<S1>:1159' */
23722 saturatedUnaryMinus = motor_Y.JD_Error;
23723 if (saturatedUnaryMinus < 0) {
23724 if (saturatedUnaryMinus <= MIN_int32_T) {
23725 saturatedUnaryMinus = MAX_int32_T;
23726 } else {
23727 saturatedUnaryMinus = -saturatedUnaryMinus;
23728 }
23729 }
23730
23731 if (saturatedUnaryMinus <= 200) {
23732 /* Transition: '<S1>:1161' */
23733 motor_DWork.is_Motor_check = motor_IN_defult4;
23734 motor_DWork.temporalCounter_i8 = 0U;
23735 }
23736 break;
23737
23738 default:
23739 /* During 'defult4': '<S1>:1324' */
23740 if (motor_DWork.temporalCounter_i8 >= (uint32_T)(5.0 / motor_DWork.Ts)) {
23741 /* Transition: '<S1>:1325' */
23742 motor_DWork.is_Motor_check = motor_IN_defult_bq;
23743
23744 /* Entry 'defult': '<S1>:1151' */
23745 motor_Y.Flag_Motor_Error = 1U;
23746 } else {
23747 saturatedUnaryMinus = motor_Y.JD_Error;
23748 if (saturatedUnaryMinus < 0) {
23749 if (saturatedUnaryMinus <= MIN_int32_T) {
23750 saturatedUnaryMinus = MAX_int32_T;
23751 } else {
23752 saturatedUnaryMinus = -saturatedUnaryMinus;
23753 }
23754 }
23755
23756 if (saturatedUnaryMinus > 200) {
23757 /* Transition: '<S1>:1326' */
23758 motor_DWork.is_Motor_check = motor_IN_defult3;
23759 }
23760 }
23761 break;
23762 }
23763}
23764
23765/* Model step function */
23766void motor_step(void)
23767{
23768 /* Chart: '<Root>/Chart' incorporates:
23769 * Inport: '<Root>/Encode_Sp'
23770 * Inport: '<Root>/Motor_Num'
23771 * Inport: '<Root>/System_Order'
23772 */
23773 /* Gateway: Chart */
23774 if (motor_DWork.temporalCounter_i1 < MAX_uint32_T) {
23775 motor_DWork.temporalCounter_i1++;
23776 }
23777
23778 if (motor_DWork.temporalCounter_i2 < MAX_uint32_T) {
23779 motor_DWork.temporalCounter_i2++;
23780 }
23781
23782 if (motor_DWork.temporalCounter_i3 < MAX_uint32_T) {
23783 motor_DWork.temporalCounter_i3++;
23784 }
23785
23786 if (motor_DWork.temporalCounter_i4 < MAX_uint32_T) {
23787 motor_DWork.temporalCounter_i4++;
23788 }
23789
23790 if (motor_DWork.temporalCounter_i5 < MAX_uint32_T) {
23791 motor_DWork.temporalCounter_i5++;
23792 }
23793
23794 if (motor_DWork.temporalCounter_i6 < 5.0 / motor_DWork.Ts) {
23795 motor_DWork.temporalCounter_i6++;
23796 }
23797
23798 if (motor_DWork.temporalCounter_i7 < MAX_uint32_T) {
23799 motor_DWork.temporalCounter_i7++;
23800 }
23801
23802 if (motor_DWork.temporalCounter_i8 < MAX_uint32_T) {
23803 motor_DWork.temporalCounter_i8++;
23804 }
23805
23806 if (motor_DWork.temporalCounter_i9 < MAX_uint32_T) {
23807 motor_DWork.temporalCounter_i9++;
23808 }
23809
23810 /* During: Chart */
23811 if (motor_DWork.is_active_c3_motor == 0U) {
23812 /* Entry: Chart */
23813 motor_DWork.is_active_c3_motor = 1U;
23814
23815 /* Entry Internal: Chart */
23816 /* Transition: '<S1>:561' */
23817 motor_DWork.is_c3_motor = motor_IN_Ready;
23818
23819 /* Entry 'Ready': '<S1>:1' */
23820 motor_DWork.Cur_Limit = 2250U;
23821
23822 /* 电流报警阈值 */
23823 motor_DWork.Cur_GZ_Limit = 2450U;
23824
23825 /* 电流故障阈值 */
23826 motor_DWork.Cur_Limit_Down = 1650.0;
23827
23828 /* 电流报警阈值 */
23829 motor_DWork.Cur_GZ_Limit_Down = 1450.0;
23830
23831 /* 电流故障阈值 */
23832 motor_DWork.Temp_Up_Limit = 2130U;
23833
23834 /* 温度上限阈值80°C */
23835 motor_DWork.Temp_Down_Limit = 80U;
23836
23837 /* Outport: '<Root>/Open_Result' */
23838 /* 温度下限阈值-45°C */
23839 motor_Y.Open_Result = 2U;
23840
23841 /* Outport: '<Root>/Ini_Result' */
23842 /* 开机结果失败 */
23843 motor_Y.Ini_Result = 2U;
23844
23845 /* Outport: '<Root>/dSpace_Init' */
23846 /* 初始化结果 0失败 1成功 2未完成 */
23847 motor_Y.dSpace_Init = 0U;
23848
23849 /* Outport: '<Root>/QDQ_BJ' */
23850 motor_Y.QDQ_BJ = 1U;
23851
23852 /* 默认驱动器正常,1正常,2报警,3故障 */
23853 motor_DWork.In_State = 0U;
23854
23855 /* Outport: '<Root>/QDQ_XH' */
23856 /* 状态标识,1表示初始化状态,2开机状态,3关机状态 */
23857 motor_Y.QDQ_XH = false;
23858
23859 /* Outport: '<Root>/QDQ_HY' */
23860 /* QDQ_XH引脚状态,低 */
23861 motor_Y.QDQ_HY = true;
23862
23863 /* Outport: '<Root>/Motor_En' */
23864 /* QDQ_HY引脚状态,高 */
23865 motor_Y.Motor_En = true;
23866
23867 /* Outport: '<Root>/DCZD' */
23868 /* 电机使能引脚置高 */
23869 motor_Y.DCZD = true;
23870
23871 /* 电磁制动引脚拉高,关电制动,低有效 */
23872 motor_DWork.KG_clc = 0U;
23873 motor_DWork.XiaoDaShen = 0.0;
23874
23875 /* Outport: '<Root>/Alarm_Cunt' */
23876 /* 限位测试不处理过限位报警 */
23877 motor_Y.Alarm_Cunt = 1U;
23878
23879 /* Outport: '<Root>/Flag_Up_limit' */
23880 /* 实现程序中主动上报只执行发送一次 */
23881 motor_Y.Flag_Up_limit = 1U;
23882
23883 /* Outport: '<Root>/Flag_Down_limit' */
23884 /* 上限位标识,1正常,0报警 */
23885 motor_Y.Flag_Down_limit = 1U;
23886
23887 /* Outport: '<Root>/Flag_Cur' */
23888 /* 下限位标识,1正常,0报警 */
23889 motor_Y.Flag_Cur = 1U;
23890
23891 /* Outport: '<Root>/Flag_Temp_Up' */
23892 /* 电流报警 */
23893 motor_Y.Flag_Temp_Up = 1U;
23894
23895 /* Outport: '<Root>/Flag_GXDY_Error' */
23896 /* 温度过高标识 */
23897 motor_Y.Flag_GXDY_Error = 1U;
23898
23899 /* Outport: '<Root>/Flag_Enc_Error' */
23900 /* 惯性单元数据异常标识 */
23901 motor_Y.Flag_Enc_Error = 1U;
23902
23903 /* Outport: '<Root>/Flag_Motor_Error' */
23904 /* 编码器标识,1正常,0故障 */
23905 motor_Y.Flag_Motor_Error = 1U;
23906
23907 /* Outport: '<Root>/Flag_GZ_Cur' */
23908 /* 电机正反转标识,1正常,0故障 */
23909 motor_Y.Flag_GZ_Cur = 1U;
23910
23911 /* Outport: '<Root>/Flag_Down_GZ_limit' */
23912 /* 电流过高故障 */
23913 motor_Y.Flag_Down_GZ_limit = 1U;
23914
23915 /* Outport: '<Root>/Flag_Up_GZ_limit' */
23916 /* 下限位开关故障标识,1正常,0故障 */
23917 motor_Y.Flag_Up_GZ_limit = 1U;
23918
23919 /* Outport: '<Root>/Flag_Temp_Down' */
23920 /* 上限位开关故障标识,1正常,0故障 */
23921 motor_Y.Flag_Temp_Down = 1U;
23922
23923 /* Outport: '<Root>/Flag_GXDY_IT' */
23924 /* 温度过低标识,1正常,0故障 */
23925 motor_Y.Flag_GXDY_IT = 1U;
23926
23927 /* 惯性单元通讯中断标识 */
23928 motor_DWork.EN_extern = 0U;
23929 motor_DWork.Forword = 1U;
23930
23931 /* 电机初始化上行PWM值 */
23932 /* 电机初始化下行PWM值 */
23933 motor_DWork.PWM_Value_Mid = 2500U;
23934
23935 /* PWM中值 */
23936 motor_DWork.Encode_Sp_Max = 3000U;
23937
23938 /* 换算编码器测速最大值 */
23939 motor_DWork.Encode_Sp_Min = 2U;
23940
23941 /* 编码器测速最小值 */
23942 motor_DWork.Encode_Pos1 = -1161718;
23943
23944 /* 下滑纵摇XHZY下机械限位位置 */
23945 motor_DWork.Encode_Pos2 = -1155272;
23946
23947 /* 下滑横摇XHHY下机械限位位置 */
23948 motor_DWork.Encode_Pos3 = -1173043;
23949
23950 /* 横摇灯杆XYDG下机械限位位置 */
23951 motor_DWork.Encode_Pos_Zero = 1000U;
23952
23953 /* 零点检测,回到零点时的范围 */
23954 /* 最小行程,检测电机是否正常运转 */
23955 motor_DWork.JD_Max = 20000U;
23956
23957 /* 角度变化最大值, */
23958 motor_DWork.Ts = 0.002;
23959
23960 /* 采样时间 */
23961 /* Entry Internal 'Ready': '<S1>:1' */
23962 /* Transition: '<S1>:1246' */
23963 if (motor_U.Motor_Num == 1) {
23964 /* Transition: '<S1>:1247' */
23965 motor_DWork.is_Ready = motor_IN_PID_parameters1;
23966
23967 /* Entry 'PID_parameters1': '<S1>:1242' */
23968 motor_DWork.P_KP = 380.0;
23969 motor_DWork.V_KP = 1.0;
23970 motor_DWork.V_KI = 10.0;
23971 } else if (motor_U.Motor_Num == 3) {
23972 /* Transition: '<S1>:1248' */
23973 motor_DWork.is_Ready = motor_IN_PID_parameters2;
23974
23975 /* Entry 'PID_parameters2': '<S1>:1243' */
23976 motor_DWork.P_KP = 330.0;
23977 motor_DWork.V_KP = 1.0;
23978 motor_DWork.V_KI = 10.0;
23979 } else {
23980 if (motor_U.Motor_Num == 2) {
23981 /* Transition: '<S1>:1249' */
23982 motor_DWork.is_Ready = motor_IN_PID_parameters3;
23983
23984 /* Entry 'PID_parameters3': '<S1>:1244' */
23985 motor_DWork.P_KP = 1200.0;
23986 motor_DWork.V_KP = 0.08;
23987 motor_DWork.V_KI = 0.8;
23988 }
23989 }
23990 } else if (motor_DWork.is_c3_motor == motor_IN_Ready) {
23991 /* During 'Ready': '<S1>:1' */
23992 if (motor_U.System_Order == 1) {
23993 /* Transition: '<S1>:568' */
23994 /* Exit Internal 'Ready': '<S1>:1' */
23995 motor_DWork.is_Ready = motor_IN_NO_ACTIVE_CHILD;
23996 motor_DWork.is_c3_motor = motor_IN_Ready_Run;
23997
23998 /* Entry Internal 'Ready_Run': '<S1>:2' */
23999 motor_DWork.is_active_Current_check = 1U;
24000
24001 /* Entry Internal 'Current_check': '<S1>:3' */
24002 /* Transition: '<S1>:369' */
24003 motor_DWork.is_Current_check = motor_IN_p1;
24004
24005 /* Entry 'p1': '<S1>:4' */
24006 motor_DWork.cur_i = 0U;
24007
24008 /* Outport: '<Root>/Flag_Cur' */
24009 /* 计数器置零 */
24010 motor_Y.Flag_Cur = 1U;
24011
24012 /* 电流正常 */
24013 motor_DWork.is_active_Temp_check = 1U;
24014 motor_DWork.is_active_M_Run = 1U;
24015
24016 /* Entry 'M_Run': '<S1>:11' */
24017 motor_DWork.Encode_Pos0 += (real_T)motor_U.Encode_Sp;
24018
24019 /* Entry Internal 'M_Run': '<S1>:11' */
24020 /* Transition: '<S1>:384' */
24021 motor_DWork.is_M_Run = motor_IN_Initialize;
24022
24023 /* Outport: '<Root>/Ini_Result' incorporates:
24024 * Inport: '<Root>/Encode_Sp'
24025 */
24026 /* Entry 'Initialize': '<S1>:12' */
24027 motor_Y.Ini_Result = 2U;
24028
24029 /* 初始化未完成 */
24030 motor_DWork.In_State = 1U;
24031
24032 /* Entry Internal 'Initialize': '<S1>:12' */
24033 /* Transition: '<S1>:396' */
24034 motor_DWork.is_Initialize = motor_IN_Parameters_Reset0;
24035 motor_DWork.temporalCounter_i1 = 0U;
24036
24037 /* Outport: '<Root>/DCZD' */
24038 /* Entry 'Parameters_Reset0': '<S1>:89' */
24039 motor_Y.DCZD = false;
24040
24041 /* 解除制动 */
24042 motor_DWork.is_active_ZHI_S_D = 1U;
24043
24044 /* Entry Internal 'ZHI_S_D': '<S1>:60' */
24045 /* Transition: '<S1>:446' */
24046 motor_DWork.is_ZHI_S_D = motor_IN_defult_b;
24047
24048 /* Outport: '<Root>/QDQ_BJ' */
24049 /* Entry 'defult': '<S1>:70' */
24050 motor_Y.QDQ_BJ = 1U;
24051
24052 /* Outport: '<Root>/Alarm_Cunt' */
24053 motor_Y.Alarm_Cunt = 1U;
24054 motor_DWork.is_active_Fault_Exit = 1U;
24055
24056 /* Entry 'Fault_Exit': '<S1>:1100' */
24057 /* 解除故障 */
24058 /* Entry Internal 'Fault_Exit': '<S1>:1100' */
24059 motor_DWork.is_active_Limit_Fault_Exit = 1U;
24060
24061 /* Entry Internal 'Limit_Fault_Exit': '<S1>:1128' */
24062 /* Transition: '<S1>:1101' */
24063 motor_DWork.is_Limit_Fault_Exit = motor_IN_defult_bq;
24064 motor_DWork.is_active_Motor_check = 1U;
24065
24066 /* Entry 'Motor_check': '<S1>:1144' */
24067 /* 电机故障诊断 */
24068 /* Entry Internal 'Motor_check': '<S1>:1144' */
24069 /* Transition: '<S1>:1146' */
24070 motor_DWork.is_Motor_check = motor_IN_defult_bq;
24071
24072 /* Outport: '<Root>/Flag_Motor_Error' */
24073 /* Entry 'defult': '<S1>:1151' */
24074 motor_Y.Flag_Motor_Error = 1U;
24075 }
24076 } else {
24077 motor_Ready_Run();
24078 }
24079
24080 if (5.0 / motor_DWork.Ts == motor_DWork.temporalCounter_i6) {
24081 motor_DWork.temporalCounter_i6 = 0U;
24082 }
24083
24084 /* End of Chart: '<Root>/Chart' */
24085
24086 /* Update absolute time for base rate */
24087 /* The "clockTick0" counts the number of times the code of this task has
24088 * been executed. The resolution of this integer timer is 0.002, which is the step size
24089 * of the task. Size of "clockTick0" ensures timer will not overflow during the
24090 * application lifespan selected.
24091 * Timer of this task consists of two 32 bit unsigned integers.
24092 * The two integers represent the low bits Timing.clockTick0 and the high bits
24093 * Timing.clockTickH0. When the low bit overflows to 0, the high bits increment.
24094 */
24095 motor_M->Timing.clockTick0++;
24096 if (!motor_M->Timing.clockTick0) {
24097 motor_M->Timing.clockTickH0++;
24098 }
24099}
24100
24101/* Model initialize function */
24102void motor_initialize(void)
24103{
24104 /* Registration code */
24105
24106 /* initialize real-time model */
24107 (void) memset((void *)motor_M, 0,
24108 sizeof(RT_MODEL_motor));
24109
24110 /* block I/O */
24111 (void) memset(((void *) &motor_B), 0,
24112 sizeof(BlockIO_motor));
24113
24114 {
24115 motor_B.y = 0.0;
24116 motor_B.In = 0.0;
24117 motor_B.Showing_Angle = 0.0;
24118 motor_B.Showing_T = 0.0;
24119 motor_B.SFunction_o52 = 0.0;
24120 motor_B.SFunction_o53 = 0.0;
24121 motor_B.SFunction_o54 = 0.0;
24122 motor_B.SFunction_o55 = 0.0;
24123 motor_B.Showing_Angle_p = 0.0;
24124 motor_B.Showing_T_a = 0.0;
24125 motor_B.DataTypeConversion2 = 0.0;
24126 motor_B.GXZ6 = 0.0;
24127 motor_B.GXZ1 = 0.0;
24128 motor_B.UnitDelay = 0.0;
24129 motor_B.Add = 0.0;
24130 motor_B.Abs = 0.0;
24131 motor_B.PulseGenerator = 0.0;
24132 motor_B.Product = 0.0;
24133 motor_B.DiscreteTimeIntegrator = 0.0;
24134 motor_B.Sum1 = 0.0;
24135 motor_B.ZV_Kp = 0.0;
24136 motor_B.ZV_Kpt = 0.0;
24137 motor_B.ZV_Kp1 = 0.0;
24138 motor_B.ZV_KD = 0.0;
24139 motor_B.DiscreteTimeIntegrator1 = 0.0;
24140 motor_B.Sum = 0.0;
24141 motor_B.ZV_KN = 0.0;
24142 motor_B.Saturation = 0.0;
24143 motor_B.KD_KG = 0.0;
24144 motor_B.Sum2 = 0.0;
24145 motor_B.ZXF_PID = 0.0;
24146 motor_B.Subtract = 0.0;
24147 motor_B.ZXF_PWM = 0.0;
24148 motor_B.ZV_Ki = 0.0;
24149 motor_B.Showing_XHHY.MultiportSwitch = 0.0;
24150 motor_B.Showing_XHHY.DataStoreRead = 0.0;
24151 motor_B.Showing_XHHY.Product = 0.0;
24152 motor_B.Showing_XHHY.Gain = 0.0;
24153 motor_B.Showing_XHHY.DataTypeConversion1 = 0.0;
24154 motor_B.Showing_XHHY.SineWave1 = 0.0;
24155 motor_B.Showing_XHHY.SineWave2 = 0.0;
24156 motor_B.Showing_XHHY.SineWave3 = 0.0;
24157 motor_B.Showing_XHHY.SineWave4 = 0.0;
24158 motor_B.Showing_XHHY.SineWave5 = 0.0;
24159 motor_B.Showing_XHHY.SineWave6 = 0.0;
24160 motor_B.Showing_XHHY.SineWave7 = 0.0;
24161 motor_B.Showing_XHHY.SineWave8 = 0.0;
24162 motor_B.Waveform_Build.Delay5 = 0.0;
24163 motor_B.Waveform_Build.DataTypeConversion4 = 0.0;
24164 motor_B.Waveform_Build.Gain1 = 0.0;
24165 motor_B.Waveform_Build.XF_XHZY = 0.0;
24166 motor_B.Waveform_Build.Add16 = 0.0;
24167 motor_B.Waveform_Build.Delay7 = 0.0;
24168 motor_B.Waveform_Build.DataStoreRead1 = 0.0;
24169 motor_B.Waveform_Build.Delay6 = 0.0;
24170 motor_B.Waveform_Build.DataStoreRead4 = 0.0;
24171 motor_B.Waveform_Build.Add21 = 0.0;
24172 motor_B.Waveform_Build.Switch = 0.0;
24173 motor_B.Waveform_Build.Add20 = 0.0;
24174 motor_B.Waveform_Build.Abs2 = 0.0;
24175 motor_B.Waveform_Build.DataStoreRead5 = 0.0;
24176 motor_B.Waveform_Build.Divide1 = 0.0;
24177 motor_B.Waveform_Build.Saturation2 = 0.0;
24178 motor_B.Waveform_Build.Add13 = 0.0;
24179 motor_B.Waveform_Build.Add15 = 0.0;
24180 motor_B.Waveform_Build.Divide = 0.0;
24181 motor_B.Waveform_Build.Product1 = 0.0;
24182 motor_B.Waveform_Build.Saturation1 = 0.0;
24183 motor_B.Waveform_Build.sampletime = 0.0;
24184 motor_B.Waveform_Build.deltariselimit = 0.0;
24185 motor_B.Waveform_Build.Yk1 = 0.0;
24186 motor_B.Waveform_Build.UkYk1 = 0.0;
24187 motor_B.Waveform_Build.Gain3 = 0.0;
24188 motor_B.Waveform_Build.deltafalllimit = 0.0;
24189 motor_B.Waveform_Build.Switch_c = 0.0;
24190 motor_B.Waveform_Build.Switch2 = 0.0;
24191 motor_B.Waveform_Build.DifferenceInputs2 = 0.0;
24192 motor_B.Waveform_Build.Switch_e = 0.0;
24193 motor_B.Waveform_Build.Switch2_p = 0.0;
24194 motor_B.Waveform_Build.In1 = 0.0;
24195 motor_B.Waveform_Build.In1_n = 0.0;
24196 motor_B.Waveform_Build.DataStoreRead5_m = 0.0;
24197 motor_B.Waveform_Build.Add20_j = 0.0;
24198 motor_B.Waveform_Build.Saturation = 0.0;
24199 motor_B.Waveform_Build.DataStoreRead5_p = 0.0;
24200 motor_B.Waveform_Build.In1_m = 0.0;
24201 motor_B.Waveform_Build.y1 = 0.0;
24202 motor_B.Waveform_Build.y2 = 0.0;
24203 motor_B.Waveform_Build.Gain7 = 0.0;
24204 motor_B.Waveform_Build.Add12 = 0.0;
24205 motor_B.Waveform_Build.Add19 = 0.0;
24206 motor_B.Angle_Calculation_HY.DataTypeConversion1 = 0.0;
24207 motor_B.Angle_Calculation_HY.GHDG5 = 0.0;
24208 motor_B.Angle_Calculation_HY.GHDG7 = 0.0;
24209 motor_B.Angle_Calculation_HY.GHDG8 = 0.0;
24210 motor_B.Angle_Calculation_HY.Fcn7 = 0.0;
24211 motor_B.Angle_Calculation_HY.Cl1 = 0.0;
24212 motor_B.Angle_Calculation_HY.Fcn = 0.0;
24213 motor_B.Angle_Calculation_HY.Fcn4 = 0.0;
24214 motor_B.Angle_Calculation_HY.GHDG9 = 0.0;
24215 motor_B.Showing.MultiportSwitch = 0.0;
24216 motor_B.Showing.DataStoreRead = 0.0;
24217 motor_B.Showing.Product = 0.0;
24218 motor_B.Showing.DataTypeConversion1 = 0.0;
24219 motor_B.Showing.SineWave1 = 0.0;
24220 motor_B.Showing.SineWave2 = 0.0;
24221 motor_B.Showing.SineWave3 = 0.0;
24222 motor_B.Showing.SineWave4 = 0.0;
24223 motor_B.Showing.SineWave5 = 0.0;
24224 motor_B.Showing.SineWave6 = 0.0;
24225 motor_B.Showing.SineWave7 = 0.0;
24226 motor_B.Showing.SineWave8 = 0.0;
24227 motor_B.Motor_HYDG1.DiscreteTimeIntegrator = 0.0;
24228 motor_B.Motor_HYDG1.chu_jd = 0.0;
24229 motor_B.Motor_HYDG1.Product = 0.0;
24230 motor_B.Motor_HYDG1.Sum4 = 0.0;
24231 motor_B.Motor_HYDG1.GHDG2 = 0.0;
24232 motor_B.Motor_HYDG1.Fcn = 0.0;
24233 motor_B.Motor_HYDG1.Fcn1 = 0.0;
24234 motor_B.Motor_HYDG1.Fcn2 = 0.0;
24235 motor_B.Motor_HYDG1.Fcn3 = 0.0;
24236 motor_B.Motor_HYDG1.Sum1 = 0.0;
24237 motor_B.Motor_HYDG1.Fcn4 = 0.0;
24238 motor_B.Motor_HYDG1.Fcn15 = 0.0;
24239 motor_B.Motor_HYDG1.GHDG3 = 0.0;
24240 motor_B.Motor_HYDG1.GHDG4 = 0.0;
24241 motor_B.Motor_HYDG1.DataTypeConversion1 = 0.0;
24242 motor_B.Motor_HYDG1.GHDG5 = 0.0;
24243 motor_B.Motor_HYDG1.fk_dg1 = 0.0;
24244 motor_B.Motor_HYDG1.fk_dg = 0.0;
24245 motor_B.Motor_HYDG1.Sum = 0.0;
24246 motor_B.Motor_HYDG1.Saturation1 = 0.0;
24247 motor_B.Motor_HYDG1.ZP_Kp = 0.0;
24248 motor_B.Motor_HYDG1.ZV_KD1 = 0.0;
24249 motor_B.Motor_HYDG1.Sum_i = 0.0;
24250 motor_B.Motor_HYDG1.ZP_KN = 0.0;
24251 motor_B.Motor_HYDG1.KD_KG = 0.0;
24252 motor_B.Motor_HYDG1.P_KP = 0.0;
24253 motor_B.Motor_HYDG1.Product_i = 0.0;
24254 motor_B.Motor_HYDG1.Sum1_n = 0.0;
24255 motor_B.Motor_HYDG1.DiscreteTimeIntegrator_d = 0.0;
24256 motor_B.Motor_HYDG1.DiscreteTimeIntegrator1 = 0.0;
24257 motor_B.Motor_HYDG1.Saturation_limit_speed = 0.0;
24258 motor_B.Motor_HYDG1.KP_e2 = 0.0;
24259 motor_B.Motor_HYDG1.Switch = 0.0;
24260 motor_B.Motor_HYDG1.Switch2 = 0.0;
24261 motor_B.Motor_HYDG1.UnitDelay = 0.0;
24262 motor_B.Motor_HYDG1.Sum5 = 0.0;
24263 motor_B.Motor_HYDG1.GXZ5 = 0.0;
24264 motor_B.Motor_HYDG1.fk_dg2 = 0.0;
24265 motor_B.Motor_HYDG1.Sum2 = 0.0;
24266 motor_B.Motor_HYDG1.ZV_Kp = 0.0;
24267 motor_B.Motor_HYDG1.ZV_Kp1 = 0.0;
24268 motor_B.Motor_HYDG1.Sum_o = 0.0;
24269 motor_B.Motor_HYDG1.ZV_KN = 0.0;
24270 motor_B.Motor_HYDG1.KD_KG_h = 0.0;
24271 motor_B.Motor_HYDG1.V_KP = 0.0;
24272 motor_B.Motor_HYDG1.Product_i4 = 0.0;
24273 motor_B.Motor_HYDG1.V_KI = 0.0;
24274 motor_B.Motor_HYDG1.Product1 = 0.0;
24275 motor_B.Motor_HYDG1.Sign = 0.0;
24276 motor_B.Motor_HYDG1.ZV1_Kp1 = 0.0;
24277 motor_B.Motor_HYDG1.ZV1_Kp2 = 0.0;
24278 motor_B.Motor_HYDG1.Sum2_f = 0.0;
24279 motor_B.Motor_HYDG1.Sum1_ne = 0.0;
24280 motor_B.Motor_HYDG1.XF_PID = 0.0;
24281 motor_B.Motor_HYDG1.Sum3 = 0.0;
24282 motor_B.Motor_HYDG1.XF_PWM = 0.0;
24283 motor_B.Motor_HYDG1.GHDG7 = 0.0;
24284 motor_B.Motor_HYDG1.GHDG8 = 0.0;
24285 motor_B.Motor_HYDG1.Fcn7 = 0.0;
24286 motor_B.Motor_HYDG1.Cl1 = 0.0;
24287 motor_B.Motor_HYDG1.Fcn_l = 0.0;
24288 motor_B.Motor_HYDG1.Fcn4_g = 0.0;
24289 motor_B.Motor_HYDG1.GHDG9 = 0.0;
24290 motor_B.Motor_HYDG1.fk_dg3 = 0.0;
24291 motor_B.Motor_HYDG1.KP_JD = 0.0;
24292 motor_B.Motor_HYDG1.Sum1_h = 0.0;
24293 motor_B.Motor_HYDG1.KP_e = 0.0;
24294 motor_B.IfActionSubsystem2.IfActionSubsystem1.PulseGenerator = 0.0;
24295 motor_B.IfActionSubsystem2.IfActionSubsystem.PulseGenerator = 0.0;
24296 motor_B.IfActionSubsystem1_m.PulseGenerator = 0.0;
24297 motor_B.IfActionSubsystem_f.PulseGenerator = 0.0;
24298 motor_B.IfActionSubsystem.IfActionSubsystem1.PulseGenerator = 0.0;
24299 motor_B.IfActionSubsystem.IfActionSubsystem.PulseGenerator = 0.0;
24300 motor_B.Motor_XHZY.chu_jd = 0.0;
24301 motor_B.Motor_XHZY.DataTypeConversion3 = 0.0;
24302 motor_B.Motor_XHZY.GXZ2 = 0.0;
24303 motor_B.Motor_XHZY.XF_YJ = 0.0;
24304 motor_B.Motor_XHZY.Product2 = 0.0;
24305 motor_B.Motor_XHZY.DataStoreRead2 = 0.0;
24306 motor_B.Motor_XHZY.DataStoreRead3 = 0.0;
24307 motor_B.Motor_XHZY.DataTypeConversion1 = 0.0;
24308 motor_B.Motor_XHZY.GXZ6 = 0.0;
24309 motor_B.Motor_XHZY.GXZ1 = 0.0;
24310 motor_B.Motor_XHZY.GXZ9 = 0.0;
24311 motor_B.Motor_XHZY.Product = 0.0;
24312 motor_B.Motor_XHZY.GXZ7 = 0.0;
24313 motor_B.Motor_XHZY.Fcn1 = 0.0;
24314 motor_B.Motor_XHZY.Fcn2 = 0.0;
24315 motor_B.Motor_XHZY.Fcn3 = 0.0;
24316 motor_B.Motor_XHZY.Fcn6 = 0.0;
24317 motor_B.Motor_XHZY.GXZ8 = 0.0;
24318 motor_B.Motor_XHZY.Product3 = 0.0;
24319 motor_B.Motor_XHZY.Sum2 = 0.0;
24320 motor_B.Motor_XHZY.Sum = 0.0;
24321 motor_B.Motor_XHZY.JD_ek = 0.0;
24322 motor_B.Motor_XHZY.CDB_XHHY = 0.0;
24323 motor_B.Motor_XHZY.DiscreteTimeIntegrator = 0.0;
24324 motor_B.Motor_XHZY.P_KP = 0.0;
24325 motor_B.Motor_XHZY.Product_k = 0.0;
24326 motor_B.Motor_XHZY.ZP_Kp2 = 0.0;
24327 motor_B.Motor_XHZY.ZV_KD1 = 0.0;
24328 motor_B.Motor_XHZY.Sum_e = 0.0;
24329 motor_B.Motor_XHZY.ZP_KN = 0.0;
24330 motor_B.Motor_XHZY.KD_KG = 0.0;
24331 motor_B.Motor_XHZY.ZP_Kp1 = 0.0;
24332 motor_B.Motor_XHZY.Sum1 = 0.0;
24333 motor_B.Motor_XHZY.DiscreteTimeIntegrator_b = 0.0;
24334 motor_B.Motor_XHZY.DiscreteTimeIntegrator1 = 0.0;
24335 motor_B.Motor_XHZY.Saturation = 0.0;
24336 motor_B.Motor_XHZY.Sum1_n = 0.0;
24337 motor_B.Motor_XHZY.ZV_Kp = 0.0;
24338 motor_B.Motor_XHZY.ZV_Kp1 = 0.0;
24339 motor_B.Motor_XHZY.ZV_KD = 0.0;
24340 motor_B.Motor_XHZY.Sum_i = 0.0;
24341 motor_B.Motor_XHZY.ZV_KN = 0.0;
24342 motor_B.Motor_XHZY.Saturation_p = 0.0;
24343 motor_B.Motor_XHZY.KD_KG_a = 0.0;
24344 motor_B.Motor_XHZY.V_KP = 0.0;
24345 motor_B.Motor_XHZY.Product_p = 0.0;
24346 motor_B.Motor_XHZY.V_KI = 0.0;
24347 motor_B.Motor_XHZY.Product1 = 0.0;
24348 motor_B.Motor_XHZY.Saturation1 = 0.0;
24349 motor_B.Motor_XHZY.Product4 = 0.0;
24350 motor_B.Motor_XHZY.Sign = 0.0;
24351 motor_B.Motor_XHZY.ZV1_Kp1 = 0.0;
24352 motor_B.Motor_XHZY.ZV1_Kp2 = 0.0;
24353 motor_B.Motor_XHZY.Sum2_h = 0.0;
24354 motor_B.Motor_XHZY.Sum1_i = 0.0;
24355 motor_B.Motor_XHZY.KP_JD1 = 0.0;
24356 motor_B.Motor_XHZY.KP_JD = 0.0;
24357 motor_B.Motor_XHZY.KP_e = 0.0;
24358 motor_B.Motor_XHZY.ZXF_PID = 0.0;
24359 motor_B.Motor_XHZY.Subtract = 0.0;
24360 motor_B.Motor_XHZY.ZXF_PWM = 0.0;
24361 motor_B.Motor_XHZY.DataTypeConversion2 = 0.0;
24362 motor_B.Motor_XHZY.GXZ3 = 0.0;
24363 motor_B.Motor_XHZY.GXZ10 = 0.0;
24364 motor_B.Motor_XHZY.XF_XHZY = 0.0;
24365 motor_B.Motor_XHZY.Product1_a = 0.0;
24366 motor_B.Motor_HYDG.chu_jd = 0.0;
24367 motor_B.Motor_HYDG.Product = 0.0;
24368 motor_B.Motor_HYDG.DataTypeConversion1 = 0.0;
24369 motor_B.Motor_HYDG.GHDG5 = 0.0;
24370 motor_B.Motor_HYDG.GHDG7 = 0.0;
24371 motor_B.Motor_HYDG.GHDG8 = 0.0;
24372 motor_B.Motor_HYDG.Fcn7 = 0.0;
24373 motor_B.Motor_HYDG.Cl1 = 0.0;
24374 motor_B.Motor_HYDG.Fcn = 0.0;
24375 motor_B.Motor_HYDG.Fcn4 = 0.0;
24376 motor_B.Motor_HYDG.GHDG9 = 0.0;
24377 motor_B.Motor_HYDG.fk_dg = 0.0;
24378 motor_B.Motor_HYDG.Sum = 0.0;
24379 motor_B.Motor_HYDG.JD_ek = 0.0;
24380 motor_B.Motor_HYDG.CDB_XHHY = 0.0;
24381 motor_B.Motor_HYDG.DiscreteTimeIntegrator = 0.0;
24382 motor_B.Motor_HYDG.ZP_Kp = 0.0;
24383 motor_B.Motor_HYDG.ZP_Kp2 = 0.0;
24384 motor_B.Motor_HYDG.ZV_KD1 = 0.0;
24385 motor_B.Motor_HYDG.Sum_g = 0.0;
24386 motor_B.Motor_HYDG.ZP_KN = 0.0;
24387 motor_B.Motor_HYDG.KD_KG = 0.0;
24388 motor_B.Motor_HYDG.ZP_Kp1 = 0.0;
24389 motor_B.Motor_HYDG.Sum1 = 0.0;
24390 motor_B.Motor_HYDG.DiscreteTimeIntegrator_o = 0.0;
24391 motor_B.Motor_HYDG.DiscreteTimeIntegrator1 = 0.0;
24392 motor_B.Motor_HYDG.Saturation = 0.0;
24393 motor_B.Motor_HYDG.Sum2 = 0.0;
24394 motor_B.Motor_HYDG.ZV_Kp = 0.0;
24395 motor_B.Motor_HYDG.ZV_Kp1 = 0.0;
24396 motor_B.Motor_HYDG.ZV_KD = 0.0;
24397 motor_B.Motor_HYDG.Sum_n = 0.0;
24398 motor_B.Motor_HYDG.ZV_KN = 0.0;
24399 motor_B.Motor_HYDG.Saturation_b = 0.0;
24400 motor_B.Motor_HYDG.KD_KG_l = 0.0;
24401 motor_B.Motor_HYDG.Sign = 0.0;
24402 motor_B.Motor_HYDG.ZV1_Kp1 = 0.0;
24403 motor_B.Motor_HYDG.ZV1_Kp2 = 0.0;
24404 motor_B.Motor_HYDG.ZV_Ki1 = 0.0;
24405 motor_B.Motor_HYDG.ZV_Kpt = 0.0;
24406 motor_B.Motor_HYDG.Sum2_e = 0.0;
24407 motor_B.Motor_HYDG.Sum1_g = 0.0;
24408 motor_B.Motor_HYDG.XF_PID = 0.0;
24409 motor_B.Motor_HYDG.Sum3 = 0.0;
24410 motor_B.Motor_HYDG.XF_PWM = 0.0;
24411 motor_B.Motor_HYDG.KP_JD = 0.0;
24412 motor_B.Motor_HYDG.KP_e = 0.0;
24413 motor_B.Motor_XHHY.chu_jd = 0.0;
24414 motor_B.Motor_XHHY.DataStoreRead1 = 0.0;
24415 motor_B.Motor_XHHY.DataStoreRead2 = 0.0;
24416 motor_B.Motor_XHHY.DataTypeConversion1 = 0.0;
24417 motor_B.Motor_XHHY.GXZ6 = 0.0;
24418 motor_B.Motor_XHHY.GXZ1 = 0.0;
24419 motor_B.Motor_XHHY.GXZ9 = 0.0;
24420 motor_B.Motor_XHHY.Product = 0.0;
24421 motor_B.Motor_XHHY.DataTypeConversion3 = 0.0;
24422 motor_B.Motor_XHHY.GXZ2 = 0.0;
24423 motor_B.Motor_XHHY.Fcn1 = 0.0;
24424 motor_B.Motor_XHHY.Fcn2 = 0.0;
24425 motor_B.Motor_XHHY.Fcn3 = 0.0;
24426 motor_B.Motor_XHHY.Fcn6 = 0.0;
24427 motor_B.Motor_XHHY.Fcn4 = 0.0;
24428 motor_B.Motor_XHHY.Fcn10 = 0.0;
24429 motor_B.Motor_XHHY.GXZ7 = 0.0;
24430 motor_B.Motor_XHHY.Fcn5 = 0.0;
24431 motor_B.Motor_XHHY.Fcn7 = 0.0;
24432 motor_B.Motor_XHHY.Fcn8 = 0.0;
24433 motor_B.Motor_XHHY.Sum3 = 0.0;
24434 motor_B.Motor_XHHY.Fcn11 = 0.0;
24435 motor_B.Motor_XHHY.Sum2 = 0.0;
24436 motor_B.Motor_XHHY.Fcn9 = 0.0;
24437 motor_B.Motor_XHHY.Product_p = 0.0;
24438 motor_B.Motor_XHHY.Fcn12 = 0.0;
24439 motor_B.Motor_XHHY.GXH7 = 0.0;
24440 motor_B.Motor_XHHY.Product3 = 0.0;
24441 motor_B.Motor_XHHY.Sum2_m = 0.0;
24442 motor_B.Motor_XHHY.Sum = 0.0;
24443 motor_B.Motor_XHHY.XF_XHZY1 = 0.0;
24444 motor_B.Motor_XHHY.CDB_XHHY = 0.0;
24445 motor_B.Motor_XHHY.DiscreteTimeIntegrator = 0.0;
24446 motor_B.Motor_XHHY.P_KP = 0.0;
24447 motor_B.Motor_XHHY.Product_h = 0.0;
24448 motor_B.Motor_XHHY.ZP_Kp2 = 0.0;
24449 motor_B.Motor_XHHY.ZV_KD1 = 0.0;
24450 motor_B.Motor_XHHY.Sum_p = 0.0;
24451 motor_B.Motor_XHHY.ZP_KN = 0.0;
24452 motor_B.Motor_XHHY.KD_KG = 0.0;
24453 motor_B.Motor_XHHY.ZP_Kp1 = 0.0;
24454 motor_B.Motor_XHHY.Sum1 = 0.0;
24455 motor_B.Motor_XHHY.DiscreteTimeIntegrator_b = 0.0;
24456 motor_B.Motor_XHHY.DiscreteTimeIntegrator1 = 0.0;
24457 motor_B.Motor_XHHY.Saturation = 0.0;
24458 motor_B.Motor_XHHY.Sum1_d = 0.0;
24459 motor_B.Motor_XHHY.ZV_Kp = 0.0;
24460 motor_B.Motor_XHHY.ZV_Kp1 = 0.0;
24461 motor_B.Motor_XHHY.ZV_KD = 0.0;
24462 motor_B.Motor_XHHY.Sum_b = 0.0;
24463 motor_B.Motor_XHHY.ZV_KN = 0.0;
24464 motor_B.Motor_XHHY.Saturation_a = 0.0;
24465 motor_B.Motor_XHHY.KD_KG_a = 0.0;
24466 motor_B.Motor_XHHY.V_KP = 0.0;
24467 motor_B.Motor_XHHY.Product_e = 0.0;
24468 motor_B.Motor_XHHY.V_KI = 0.0;
24469 motor_B.Motor_XHHY.Product1 = 0.0;
24470 motor_B.Motor_XHHY.Sign = 0.0;
24471 motor_B.Motor_XHHY.ZV1_Kp1 = 0.0;
24472 motor_B.Motor_XHHY.ZV1_Kp2 = 0.0;
24473 motor_B.Motor_XHHY.Sum2_n = 0.0;
24474 motor_B.Motor_XHHY.Sum1_du = 0.0;
24475 motor_B.Motor_XHHY.KP_JD1 = 0.0;
24476 motor_B.Motor_XHHY.KP_e = 0.0;
24477 motor_B.Motor_XHHY.ZXF_PID = 0.0;
24478 motor_B.Motor_XHHY.Subtract = 0.0;
24479 motor_B.Motor_XHHY.ZXF_PWM = 0.0;
24480 motor_B.Motor_XHHY.DataTypeConversion4 = 0.0;
24481 motor_B.Motor_XHHY.DataTypeConversion2 = 0.0;
24482 motor_B.Motor_XHHY.GXZ3 = 0.0;
24483 motor_B.Motor_XHHY.XF_XHZY = 0.0;
24484 motor_B.Motor_XHHY.Product1_l = 0.0;
24485 }
24486
24487 /* states (dwork) */
24488 (void) memset((void *)&motor_DWork, 0,
24489 sizeof(D_Work_motor));
24490 motor_DWork.UnitDelay_DSTATE = 0.0;
24491 motor_DWork.DiscreteTimeIntegrator_DSTATE = 0.0;
24492 motor_DWork.DiscreteTimeIntegrator1_DSTATE = 0.0;
24493 motor_DWork.Angle_extern = 0.0;
24494 motor_DWork.Angle_internal = 0.0;
24495 motor_DWork.P_KP = 0.0;
24496 motor_DWork.Rate_limit_speed = 0.0;
24497 motor_DWork.Saturation_limit_speed = 0.0;
24498 motor_DWork.T = 0.0;
24499 motor_DWork.T_Count = 0.0;
24500 motor_DWork.Temp1 = 0.0;
24501 motor_DWork.Temp2 = 0.0;
24502 motor_DWork.V_KI = 0.0;
24503 motor_DWork.V_KP = 0.0;
24504 motor_DWork.chu_jd = 0.0;
24505 motor_DWork.Angle_S = 0.0;
24506 motor_DWork.Ts = 0.0;
24507 motor_DWork.Encode_Pos0 = 0.0;
24508 motor_DWork.Showing_Angle0 = 0.0;
24509 motor_DWork.Showing_Angle_Last = 0.0;
24510 motor_DWork.Showing_T_Last = 0.0;
24511 motor_DWork.Average_En = 0.0;
24512 motor_DWork.XiaoDaShen = 0.0;
24513 motor_DWork.Cur_GZ_Limit_Down = 0.0;
24514 motor_DWork.Cur_Limit_Down = 0.0;
24515 motor_DWork.Sum1_DWORK1 = 0.0;
24516 motor_DWork.Waveform_Build.Delay5_DSTATE = 0.0;
24517 motor_DWork.Waveform_Build.Delay7_DSTATE = 0.0;
24518 motor_DWork.Waveform_Build.Delay6_DSTATE = 0.0;
24519 motor_DWork.Waveform_Build.DelayInput2_DSTATE = 0.0;
24520 motor_DWork.Motor_HYDG1.DiscreteTimeIntegrator_DSTATE = 0.0;
24521 motor_DWork.Motor_HYDG1.DiscreteTimeIntegrator_DSTATE_a = 0.0;
24522 motor_DWork.Motor_HYDG1.DiscreteTimeIntegrator1_DSTATE = 0.0;
24523 motor_DWork.Motor_HYDG1.UnitDelay_DSTATE = 0.0;
24524 motor_DWork.Motor_XHZY.DiscreteTimeIntegrator_DSTATE = 0.0;
24525 motor_DWork.Motor_XHZY.DiscreteTimeIntegrator_DSTATE_c = 0.0;
24526 motor_DWork.Motor_XHZY.DiscreteTimeIntegrator1_DSTATE = 0.0;
24527 motor_DWork.Motor_XHZY.Sum1_DWORK1 = 0.0;
24528 motor_DWork.Motor_HYDG.DiscreteTimeIntegrator_DSTATE = 0.0;
24529 motor_DWork.Motor_HYDG.DiscreteTimeIntegrator_DSTATE_i = 0.0;
24530 motor_DWork.Motor_HYDG.DiscreteTimeIntegrator1_DSTATE = 0.0;
24531 motor_DWork.Motor_HYDG.Sum2_DWORK1 = 0.0;
24532 motor_DWork.Motor_XHHY.DiscreteTimeIntegrator_DSTATE = 0.0;
24533 motor_DWork.Motor_XHHY.DiscreteTimeIntegrator_DSTATE_i = 0.0;
24534 motor_DWork.Motor_XHHY.DiscreteTimeIntegrator1_DSTATE = 0.0;
24535 motor_DWork.Motor_XHHY.Sum1_DWORK1 = 0.0;
24536
24537 /* external inputs */
24538 (void) memset((void *)&motor_U, 0,
24539 sizeof(ExternalInputs_motor));
24540
24541 /* external outputs */
24542 (void) memset((void *)&motor_Y, 0,
24543 sizeof(ExternalOutputs_motor));
24544
24545 {
24546 uint32_T prevT_idx_1;
24547 uint32_T prevT_idx_0;
24548
24549 /* Start for Chart: '<Root>/Chart' incorporates:
24550 * Start for SubSystem: '<S1>/BJ'
24551 */
24552
24553 /* Start for IfAction SubSystem: '<S5>/If Action Subsystem' */
24554 motor_IfActionSubsystem_p_Start(&motor_DWork.IfActionSubsystem);
24555
24556 /* End of Start for SubSystem: '<S5>/If Action Subsystem' */
24557
24558 /* Start for IfAction SubSystem: '<S5>/If Action Subsystem1' */
24559
24560 /* Start for IfAction SubSystem: '<S24>/If Action Subsystem' */
24561 motor_IfActionSubsystem_Start(&motor_DWork.IfActionSubsystem_f);
24562
24563 /* End of Start for SubSystem: '<S24>/If Action Subsystem' */
24564
24565 /* Start for IfAction SubSystem: '<S24>/If Action Subsystem1' */
24566 motor_IfActionSubsystem1_Start(&motor_DWork.IfActionSubsystem1_m);
24567
24568 /* End of Start for SubSystem: '<S24>/If Action Subsystem1' */
24569
24570 /* InitializeConditions for IfAction SubSystem: '<S24>/If Action Subsystem2' */
24571 /* InitializeConditions for DiscretePulseGenerator: '<S31>/Pulse Generator' */
24572 motor_DWork.clockTickCounter = 0;
24573
24574 /* End of InitializeConditions for SubSystem: '<S24>/If Action Subsystem2' */
24575 /* End of Start for SubSystem: '<S5>/If Action Subsystem1' */
24576
24577 /* Start for IfAction SubSystem: '<S5>/If Action Subsystem2' */
24578 motor_IfActionSubsystem_p_Start(&motor_DWork.IfActionSubsystem2);
24579
24580 /* End of Start for SubSystem: '<S5>/If Action Subsystem2' */
24581
24582 /* Start for Chart: '<Root>/Chart' incorporates:
24583 * Start for SubSystem: '<S1>/Waveform_Build'
24584 */
24585 motor_Waveform_Build_Start(&motor_DWork.Waveform_Build);
24586
24587 /* Start for DataStoreMemory: '<Root>/Angle_extern' */
24588 motor_DWork.Angle_extern = motor_P.Angle_extern_InitialValue;
24589
24590 /* Start for DataStoreMemory: '<Root>/Angle_internal' */
24591 motor_DWork.Angle_internal = motor_P.Angle_internal_InitialValue;
24592
24593 /* Start for DataStoreMemory: '<Root>/EN_extern' */
24594 motor_DWork.EN_extern = motor_P.EN_extern_InitialValue;
24595
24596 /* Start for DataStoreMemory: '<Root>/Forword' */
24597 motor_DWork.Forword = motor_P.Forword_InitialValue;
24598
24599 /* Start for DataStoreMemory: '<Root>/JD_HYDG' */
24600 motor_DWork.JD_HYDG = motor_P.JD_HYDG_InitialValue;
24601
24602 /* Start for DataStoreMemory: '<Root>/JD_HYDG_HC' */
24603 motor_DWork.JD_HYDG_HC = motor_P.JD_HYDG_HC_InitialValue;
24604
24605 /* Start for DataStoreMemory: '<Root>/JD_XHHY' */
24606 motor_DWork.JD_XHHY = motor_P.JD_XHHY_InitialValue;
24607
24608 /* Start for DataStoreMemory: '<Root>/JD_XHHY_HC' */
24609 motor_DWork.JD_XHHY_HC = motor_P.JD_XHHY_HC_InitialValue;
24610
24611 /* Start for DataStoreMemory: '<Root>/JD_XHZY' */
24612 motor_DWork.JD_XHZY = motor_P.JD_XHZY_InitialValue;
24613
24614 /* Start for DataStoreMemory: '<Root>/JD_XHZY_HC' */
24615 motor_DWork.JD_XHZY_HC = motor_P.JD_XHZY_HC_InitialValue;
24616
24617 /* Start for DataStoreMemory: '<Root>/KG' */
24618 motor_DWork.KG = motor_P.KG_InitialValue;
24619
24620 /* Start for DataStoreMemory: '<Root>/KG_En' */
24621 motor_DWork.KG_En = motor_P.KG_En_InitialValue;
24622
24623 /* Start for DataStoreMemory: '<Root>/KG_JD' */
24624 motor_DWork.KG_JD = motor_P.KG_JD_InitialValue;
24625
24626 /* Start for DataStoreMemory: '<Root>/KG_YJ' */
24627 motor_DWork.KG_YJ = motor_P.KG_YJ_InitialValue;
24628
24629 /* Start for DataStoreMemory: '<Root>/KG_clc' */
24630 motor_DWork.KG_clc = motor_P.KG_clc_InitialValue;
24631
24632 /* Start for DataStoreMemory: '<Root>/PC1' */
24633 motor_DWork.PC1 = motor_P.PC1_InitialValue;
24634
24635 /* Start for DataStoreMemory: '<Root>/PC2' */
24636 motor_DWork.PC2 = motor_P.PC2_InitialValue;
24637
24638 /* Start for DataStoreMemory: '<Root>/P_KP' */
24639 motor_DWork.P_KP = motor_P.P_KP_InitialValue;
24640
24641 /* Start for DataStoreMemory: '<Root>/Rate_limit_speed' */
24642 motor_DWork.Rate_limit_speed = motor_P.Rate_limit_speed_InitialValue;
24643
24644 /* Start for DataStoreMemory: '<Root>/Saturation_limit_speed' */
24645 motor_DWork.Saturation_limit_speed = motor_P.Saturation_limit_speed_InitialV;
24646
24647 /* Start for DataStoreMemory: '<Root>/T' */
24648 motor_DWork.T = motor_P.T_InitialValue;
24649
24650 /* Start for DataStoreMemory: '<Root>/T_Count' */
24651 motor_DWork.T_Count = motor_P.T_Count_InitialValue;
24652
24653 /* Start for DataStoreMemory: '<Root>/Temp1' */
24654 motor_DWork.Temp1 = motor_P.Temp1_InitialValue;
24655
24656 /* Start for DataStoreMemory: '<Root>/Temp2' */
24657 motor_DWork.Temp2 = motor_P.Temp2_InitialValue;
24658
24659 /* Start for DataStoreMemory: '<Root>/V_KI' */
24660 motor_DWork.V_KI = motor_P.V_KI_InitialValue;
24661
24662 /* Start for DataStoreMemory: '<Root>/V_KP' */
24663 motor_DWork.V_KP = motor_P.V_KP_InitialValue;
24664
24665 /* Start for DataStoreMemory: '<Root>/YJ_XHZY' */
24666 motor_DWork.YJ_XHZY = motor_P.YJ_XHZY_InitialValue;
24667
24668 /* Start for DataStoreMemory: '<Root>/YJ_XHZY_HC' */
24669 motor_DWork.YJ_XHZY_HC = motor_P.YJ_XHZY_HC_InitialValue;
24670
24671 /* Start for DataStoreMemory: '<Root>/chu_jd' */
24672 motor_DWork.chu_jd = motor_P.chu_jd_InitialValue;
24673
24674 /* Start for DataStoreMemory: '<Root>/chu_jd1' */
24675 motor_DWork.Angle_S = motor_P.chu_jd1_InitialValue;
24676 motor_DWork.is_Ready = motor_IN_NO_ACTIVE_CHILD;
24677 motor_DWork.is_active_Current_check = 0U;
24678 motor_DWork.is_Current_check = motor_IN_NO_ACTIVE_CHILD;
24679 motor_DWork.is_active_Fault_Exit = 0U;
24680 motor_DWork.is_active_Limit_Fault_Exit = 0U;
24681 motor_DWork.is_Limit_Fault_Exit = motor_IN_NO_ACTIVE_CHILD;
24682 motor_DWork.temporalCounter_i7 = 0U;
24683 motor_DWork.is_active_M_Run = 0U;
24684 motor_DWork.is_M_Run = motor_IN_NO_ACTIVE_CHILD;
24685 motor_DWork.is_Close = motor_IN_NO_ACTIVE_CHILD;
24686 motor_DWork.is_Algorithm = motor_IN_NO_ACTIVE_CHILD;
24687 motor_DWork.is_HYDG_Close = motor_IN_NO_ACTIVE_CHILD;
24688 motor_DWork.is_XHHY_Close = motor_IN_NO_ACTIVE_CHILD;
24689 motor_DWork.is_XHZY_Close = motor_IN_NO_ACTIVE_CHILD;
24690 motor_DWork.is_Initialize = motor_IN_NO_ACTIVE_CHILD;
24691 motor_DWork.is_hy = motor_IN_NO_ACTIVE_CHILD;
24692 motor_DWork.is_xhhy = motor_IN_NO_ACTIVE_CHILD;
24693 motor_DWork.is_xhzy = motor_IN_NO_ACTIVE_CHILD;
24694 motor_DWork.is_Normal_Mode = motor_IN_NO_ACTIVE_CHILD;
24695 motor_DWork.is_active_Enc_GZ = 0U;
24696 motor_DWork.is_active_Enc1 = 0U;
24697 motor_DWork.is_Enc1 = motor_IN_NO_ACTIVE_CHILD;
24698 motor_DWork.is_active_Enc2 = 0U;
24699 motor_DWork.is_Enc2 = motor_IN_NO_ACTIVE_CHILD;
24700 motor_DWork.temporalCounter_i6 = 0U;
24701 motor_DWork.temporalCounter_i5 = 0U;
24702 motor_DWork.is_active_RUN = 0U;
24703 motor_DWork.is_active_Algorithm = 0U;
24704 motor_DWork.is_Algorithm_l = motor_IN_NO_ACTIVE_CHILD;
24705 motor_DWork.is_HY = motor_IN_NO_ACTIVE_CHILD;
24706 motor_DWork.is_XHHY = motor_IN_NO_ACTIVE_CHILD;
24707 motor_DWork.is_XHZY = motor_IN_NO_ACTIVE_CHILD;
24708 motor_DWork.is_active_Error_Check = 0U;
24709 motor_DWork.is_Error_Check = motor_IN_NO_ACTIVE_CHILD;
24710 motor_DWork.is_Error_Check_g = motor_IN_NO_ACTIVE_CHILD;
24711 motor_DWork.temporalCounter_i9 = 0U;
24712 motor_DWork.is_active_GXDY_COM_GZ = 0U;
24713 motor_DWork.is_GXDY_COM_GZ = motor_IN_NO_ACTIVE_CHILD;
24714 motor_DWork.temporalCounter_i3 = 0U;
24715 motor_DWork.is_active_GXDY_SJ_BJ = 0U;
24716 motor_DWork.is_GXDY_SJ_BJ = motor_IN_NO_ACTIVE_CHILD;
24717 motor_DWork.is_active_Limit_Check = 0U;
24718 motor_DWork.is_Limit_Check = motor_IN_NO_ACTIVE_CHILD;
24719 motor_DWork.temporalCounter_i4 = 0U;
24720 motor_DWork.is_Showing_Mode = motor_IN_NO_ACTIVE_CHILD;
24721 motor_DWork.is_active_Algorithm_b = 0U;
24722 motor_DWork.is_Algorithm_e = motor_IN_NO_ACTIVE_CHILD;
24723 motor_DWork.is_HY_e = motor_IN_NO_ACTIVE_CHILD;
24724 motor_DWork.is_Limit_Down_Test_i = motor_IN_NO_ACTIVE_CHILD;
24725 motor_DWork.is_HY_aj = motor_IN_NO_ACTIVE_CHILD;
24726 motor_DWork.is_XHHY_fs = motor_IN_NO_ACTIVE_CHILD;
24727 motor_DWork.is_XHZY_m = motor_IN_NO_ACTIVE_CHILD;
24728 motor_DWork.is_Limit_Up_Test_j = motor_IN_NO_ACTIVE_CHILD;
24729 motor_DWork.is_HY_l = motor_IN_NO_ACTIVE_CHILD;
24730 motor_DWork.is_XHHY_nb = motor_IN_NO_ACTIVE_CHILD;
24731 motor_DWork.is_XHZY_d = motor_IN_NO_ACTIVE_CHILD;
24732 motor_DWork.is_XHHY_n = motor_IN_NO_ACTIVE_CHILD;
24733 motor_DWork.is_XHZY_h = motor_IN_NO_ACTIVE_CHILD;
24734 motor_DWork.temporalCounter_i2 = 0U;
24735 motor_DWork.is_active_Limit_Check_o = 0U;
24736 motor_DWork.is_Limit_Check_j = motor_IN_NO_ACTIVE_CHILD;
24737 motor_DWork.is_Test_Mode = motor_IN_NO_ACTIVE_CHILD;
24738 motor_DWork.is_Dspace_Test = motor_IN_NO_ACTIVE_CHILD;
24739 motor_DWork.is_Elevation_Test = motor_IN_NO_ACTIVE_CHILD;
24740 motor_DWork.is_Limit_Down_Test = motor_IN_NO_ACTIVE_CHILD;
24741 motor_DWork.is_HY_h = motor_IN_NO_ACTIVE_CHILD;
24742 motor_DWork.is_XHHY_f = motor_IN_NO_ACTIVE_CHILD;
24743 motor_DWork.is_XHZY_i = motor_IN_NO_ACTIVE_CHILD;
24744 motor_DWork.is_Limit_Up_Test = motor_IN_NO_ACTIVE_CHILD;
24745 motor_DWork.is_HY_a = motor_IN_NO_ACTIVE_CHILD;
24746 motor_DWork.is_XHHY_nl = motor_IN_NO_ACTIVE_CHILD;
24747 motor_DWork.is_XHZY_a = motor_IN_NO_ACTIVE_CHILD;
24748 motor_DWork.temporalCounter_i1 = 0U;
24749 motor_DWork.is_active_Motor_check = 0U;
24750 motor_DWork.is_Motor_check = motor_IN_NO_ACTIVE_CHILD;
24751 motor_DWork.temporalCounter_i8 = 0U;
24752 motor_DWork.is_active_Temp_check = 0U;
24753 motor_DWork.is_active_ZHI_S_D = 0U;
24754 motor_DWork.is_ZHI_S_D = motor_IN_NO_ACTIVE_CHILD;
24755 motor_DWork.is_active_c3_motor = 0U;
24756 motor_DWork.is_c3_motor = motor_IN_NO_ACTIVE_CHILD;
24757 motor_DWork.Cur_Limit = 0U;
24758 motor_DWork.Temp_Up_Limit = 0U;
24759 motor_DWork.PWM_Value_Mid = 0U;
24760 motor_DWork.Encode_Sp_Min = 0U;
24761 motor_DWork.Encode_Pos1 = 0;
24762 motor_DWork.Encode_Pos2 = 0;
24763 motor_DWork.Encode_Pos3 = 0;
24764 motor_DWork.Encode_Pos_Zero = 0U;
24765 motor_DWork.Ts = 0.0;
24766 motor_DWork.JD_Max = 0U;
24767 motor_DWork.Encode_Sp_Max = 0U;
24768 motor_DWork.cur_i = 0U;
24769 motor_DWork.Temp_Down_Limit = 0U;
24770 motor_DWork.In_State = 0U;
24771 motor_DWork.Enc_i = 0U;
24772 motor_DWork.can1_i = 0U;
24773 motor_DWork.GXDY_i = 0U;
24774 motor_DWork.Cur_GZ_Limit = 0U;
24775 motor_DWork.Encode_Pos0 = 0.0;
24776 motor_DWork.Showing_Angle0 = 0.0;
24777 motor_DWork.Showing_Angle_Last = 0.0;
24778 motor_DWork.Showing_T_Last = 0.0;
24779 motor_DWork.Average_En = 0.0;
24780 motor_DWork.XiaoDaShen = 0.0;
24781 motor_DWork.Cur_GZ_Limit_Down = 0.0;
24782 motor_DWork.Cur_Limit_Down = 0.0;
24783 motor_DWork.Runing_stable = 0U;
24784
24785 /* InitializeConditions for Outport: '<Root>/Ini_Result' */
24786 motor_Y.Ini_Result = 0U;
24787
24788 /* InitializeConditions for Outport: '<Root>/Flag_Cur' */
24789 motor_Y.Flag_Cur = 0U;
24790
24791 /* InitializeConditions for Outport: '<Root>/Motor_En' */
24792 motor_Y.Motor_En = false;
24793
24794 /* InitializeConditions for Outport: '<Root>/Flag_Temp_Up' */
24795 motor_Y.Flag_Temp_Up = 0U;
24796
24797 /* InitializeConditions for Outport: '<Root>/Open_Result' */
24798 motor_Y.Open_Result = 0U;
24799
24800 /* InitializeConditions for Outport: '<Root>/Encode_Pos' */
24801 motor_Y.Encode_Pos = 0;
24802
24803 /* InitializeConditions for Outport: '<Root>/PWMOUT' */
24804 motor_Y.PWMOUT = 0U;
24805
24806 /* InitializeConditions for Outport: '<Root>/Flag_Temp_Down' */
24807 motor_Y.Flag_Temp_Down = 0U;
24808
24809 /* InitializeConditions for Outport: '<Root>/QDQ_BJ' */
24810 motor_Y.QDQ_BJ = 0U;
24811
24812 /* InitializeConditions for Outport: '<Root>/QDQ_XH' */
24813 motor_Y.QDQ_XH = true;
24814
24815 /* InitializeConditions for Outport: '<Root>/QDQ_HY' */
24816 motor_Y.QDQ_HY = true;
24817
24818 /* InitializeConditions for Outport: '<Root>/Flag_Up_limit' */
24819 motor_Y.Flag_Up_limit = 0U;
24820
24821 /* InitializeConditions for Outport: '<Root>/Flag_Down_limit' */
24822 motor_Y.Flag_Down_limit = 0U;
24823
24824 /* InitializeConditions for Outport: '<Root>/Flag_Enc_Error' */
24825 motor_Y.Flag_Enc_Error = 0U;
24826
24827 /* InitializeConditions for Outport: '<Root>/Flag_Motor_Error' */
24828 motor_Y.Flag_Motor_Error = 0U;
24829
24830 /* InitializeConditions for Outport: '<Root>/DCZD' */
24831 motor_Y.DCZD = false;
24832
24833 /* InitializeConditions for Outport: '<Root>/Flag_GXDY_Error' */
24834 motor_Y.Flag_GXDY_Error = 0U;
24835
24836 /* InitializeConditions for Outport: '<Root>/Flag_GXDY_IT' */
24837 motor_Y.Flag_GXDY_IT = 0U;
24838
24839 /* InitializeConditions for Outport: '<Root>/JD_Error' */
24840 motor_Y.JD_Error = 0;
24841
24842 /* InitializeConditions for Outport: '<Root>/JD_Out' */
24843 motor_Y.JD_Out = 0;
24844
24845 /* InitializeConditions for Outport: '<Root>/SGWY_Out' */
24846 motor_Y.SGWY_Out = 0;
24847
24848 /* InitializeConditions for Outport: '<Root>/Flag_GZ_Cur' */
24849 motor_Y.Flag_GZ_Cur = 0U;
24850
24851 /* InitializeConditions for Outport: '<Root>/Flag_Up_GZ_limit' */
24852 motor_Y.Flag_Up_GZ_limit = 0U;
24853
24854 /* InitializeConditions for Outport: '<Root>/Flag_Down_GZ_limit' */
24855 motor_Y.Flag_Down_GZ_limit = 0U;
24856
24857 /* InitializeConditions for Outport: '<Root>/Alarm_Cunt' */
24858 motor_Y.Alarm_Cunt = 0U;
24859
24860 /* InitializeConditions for Outport: '<Root>/dSpace_Init' */
24861 motor_Y.dSpace_Init = 0U;
24862
24863 /* InitializeConditions for Outport: '<Root>/Flag_AngleError' */
24864 motor_Y.Flag_AngleError = 0U;
24865
24866 /* InitializeConditions for Chart: '<Root>/Chart' incorporates:
24867 * InitializeConditions for SubSystem: '<S1>/Motor_XHHY'
24868 */
24869 motor_Motor_XHHY_Init(motor_M, &motor_DWork.Motor_XHHY,
24870 (rtP_Motor_XHHY_motor *)&motor_P.Motor_XHHY);
24871
24872 /* InitializeConditions for Chart: '<Root>/Chart' incorporates:
24873 * InitializeConditions for SubSystem: '<S1>/Open_Motor'
24874 */
24875 prevT_idx_0 = motor_M->Timing.clockTick0;
24876 prevT_idx_1 = motor_M->Timing.clockTickH0;
24877 motor_DWork.Open_Motor_PREV_T[0] = prevT_idx_0;
24878 motor_DWork.Open_Motor_PREV_T[1] = prevT_idx_1;
24879
24880 /* InitializeConditions for DiscreteIntegrator: '<S66>/Discrete-Time Integrator' */
24881 motor_DWork.DiscreteTimeIntegrator_DSTATE =
24882 motor_P.DiscreteTimeIntegrator_IC;
24883 motor_DWork.DiscreteTimeIntegrator_PrevRese = 2;
24884
24885 /* InitializeConditions for DiscreteIntegrator: '<S66>/Discrete-Time Integrator1' */
24886 motor_DWork.DiscreteTimeIntegrator1_DSTATE =
24887 motor_P.DiscreteTimeIntegrator1_IC;
24888
24889 /* InitializeConditions for Chart: '<Root>/Chart' incorporates:
24890 * InitializeConditions for SubSystem: '<S1>/Motor_HYDG'
24891 */
24892 motor_Motor_HYDG_Init(motor_M, &motor_DWork.Motor_HYDG,
24893 (rtP_Motor_HYDG_motor *)&motor_P.Motor_HYDG);
24894
24895 /* InitializeConditions for Chart: '<Root>/Chart' incorporates:
24896 * InitializeConditions for SubSystem: '<S1>/Motor_XHZY'
24897 */
24898 motor_Motor_XHZY_Init(motor_M, &motor_DWork.Motor_XHZY,
24899 (rtP_Motor_XHZY_motor *)&motor_P.Motor_XHZY);
24900
24901 /* InitializeConditions for Chart: '<Root>/Chart' incorporates:
24902 * InitializeConditions for SubSystem: '<S1>/COM_Error'
24903 */
24904 /* InitializeConditions for UnitDelay: '<S6>/Unit Delay' */
24905 motor_DWork.UnitDelay_DSTATE = motor_P.UnitDelay_InitialCondition;
24906
24907 /* InitializeConditions for Chart: '<Root>/Chart' incorporates:
24908 * InitializeConditions for SubSystem: '<S1>/Motor_HYDG1'
24909 */
24910 motor_Motor_HYDG1_Init(motor_M, &motor_DWork.Motor_HYDG1,
24911 (rtP_Motor_HYDG1_motor *)&motor_P.Motor_HYDG1);
24912
24913 /* InitializeConditions for Chart: '<Root>/Chart' incorporates:
24914 * InitializeConditions for SubSystem: '<S1>/Waveform_Build'
24915 */
24916 motor_Waveform_Build_Init(motor_M, &motor_DWork.Waveform_Build,
24917 (rtP_Waveform_Build_motor *)&motor_P.Waveform_Build);
24918 }
24919}
24920
24921/* Model terminate function */
24922void motor_terminate(void)
24923{
24924 /* (no terminate code required) */
24925}
24926
24927/*
24928 * File trailer for generated code.
24929 *
24930 * [EOF]
24931 */
24932